#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <math.h> /* for cos, fabs */
#include <float.h>
#include <string.h> /* for memcpy */

#include "moab/FindPtFuncs.h"

const unsigned opt_no_constraints_2 = 3 + 1;
const unsigned opt_no_constraints_3 = 9 + 3 + 1;

/*--------------------------------------------------------------------------
   Lobatto Polynomial Bounds

   Needed inputs are the Gauss-Lobatto quadrature nodes and weights:
     unsigned nr = ..., ns = ...;
     realType zr[nr], wr[nr];
     realType zs[ns], ws[ns];

     lobatto_nodes(zr,nr); lobatto_weights(zr,wr,nr);
     lobatto_nodes(zs,ns); lobatto_weights(zs,ws,ns);

   The number of points in the constructed piecewise (bi-)linear bounds
   is a parameter; more points give tighter bounds

     unsigned mr = 2*nr, ms = 2*ns;

   The necessary setup is accomplished via:
     lob_bnd_base b_data_r;
     lob_bnd_ext  e_data_s;

     lob_bnd_base_alloc(&b_data_r,nr,mr);
     lob_bnd_base_setup(&b_data_r,zr,wr);
     lob_bnd_ext_alloc(&e_data_s,ns,ms);
     lob_bnd_ext_setup(&e_data_s,zs,ws);

   Bounds may then be computed via:
     realType work1r[2*mr], work1s[2*ms], work2[2*mr + 2*mr*ns + 2*mr*ms];
     realType ur[nr], us[ns];    // 1-d polynomials on the zr[] and zs[] nodes
     realType u[ns][nr];         // 2-d polynomial on zr[] (x) zs[]
     realType bound[2];          // = { min, max } (to be computed)

     lob_bnd_1(&b_data_r  ,ur,bound,work1r); // compute bounds on ur
     lob_bnd_1(&e_data_s.b,us,bound,work1s); // compute bounds on us
     lob_bnd_2(&b_data_r, &e_data_s,
               (const double*)&u[0][0],bound,work2); // compute bounds on u
   The above routines access the zr,zs arrays passed to *_setup
     (so do not delete them between calls)

   Memory allocated in *_setup is freed with
     lob_bnd_base_free(&b_data_r);
     lob_bnd_ext_free(&e_data_s);

  --------------------------------------------------------------------------*/

typedef struct
{
    unsigned  n; /* number of Lobatto nodes in input */
    unsigned  m; /* number of Chebyshev nodes used to calculate bounds */
    realType *Q0, *Q1; /* Q0[n], Q1[n] -- first two rows of change of basis matrix
                      from Lobatto node Lagrangian to Legendre */
    const realType* z; /* z[n] -- external; Lobatto nodes */
    realType*       h; /* h[m] -- Chebyshev nodes */
    realType *      uv, *ov; /* uv[n][m], ov[n][m] --
                              uv[j][:] is a piecewise linear function in the nodal
                                       basis with nodes h[m] that is everywhere less
                                       than or equal to the jth Lagrangian basis
                                       function (with the Lobatto nodes z[n])
                              ov[j][:] is everywhere greater than or equal */
} lob_bnd_base;

typedef struct
{
    lob_bnd_base b;
    realType *   uvp, *uvn, *ovp, *ovn; /* [n][m] -- uv and ov split into
                                         positive and negative parts */
} lob_bnd_ext;

static void lob_bnd_base_alloc( lob_bnd_base* p, unsigned n, unsigned m )
{
    p->n = n, p->m = m;
    p->Q0 = tmalloc( realType, 2 * n + m + 2 * n * m );
    p->Q1 = p->Q0 + n;
    p->h = p->Q1 + n;
    p->uv = p->h + m;
    p->ov = p->uv + n * m;
}

static void lob_bnd_base_free( lob_bnd_base* p )
{
    free( p->Q0 );
}

static void lob_bnd_ext_alloc( lob_bnd_ext* p, unsigned n, unsigned m )
{
    p->b.n = n, p->b.m = m;
    p->b.Q0 = tmalloc( realType, 2 * n + m + 6 * n * m );
    p->b.Q1 = p->b.Q0 + n;
    p->b.h = p->b.Q1 + n;
    p->b.uv = p->b.h + m;
    p->b.ov = p->b.uv + n * m;
    p->uvp = p->b.ov + n * m;
    p->uvn = p->uvp + n * m;
    p->ovp = p->uvn + n * m;
    p->ovn = p->ovp + n * m;
}

static void lob_bnd_ext_free( lob_bnd_ext* p )
{
    free( p->b.Q0 );
}

static void lob_bnd_base_setup( lob_bnd_base* p, const realType* z, const realType* w )
{
    unsigned  i, j, m = p->m, n = p->n, mm = 2 * m - 1;
    realType *q = tmalloc( realType, ( 2 * n + 1 ) * mm + 6 * n ), *J = q + mm, *D = J + n * mm, *work = D + n * mm;
    p->z = z;
    for( i = 0; i < n; ++i )
        p->Q0[ i ] = w[ i ] / 2, p->Q1[ i ] = 3 * p->Q0[ i ] * z[ i ];
    p->h[ 0 ] = -1, p->h[ m - 1 ] = 1;
    for( j = 1; j < m - 1; ++j )
        p->h[ j ] = mbcos( ( m - j - 1 ) * MOAB_POLY_PI / ( m - 1 ) );
    for( j = 0; j < m - 1; ++j )
        q[ 2 * j ] = p->h[ j ], q[ 2 * j + 1 ] = ( p->h[ j ] + p->h[ j + 1 ] ) / 2;
    q[ mm - 1 ] = p->h[ m - 1 ];
    lagrange_weights_deriv( z, n, q, mm, J, D, work );
    for( i = 0; i < n; ++i )
    {
        realType *uv = p->uv + i * m, *ov = p->ov + i * m;
        ov[ 0 ] = uv[ 0 ] = J[ i ];
        ov[ m - 1 ] = uv[ m - 1 ] = J[ ( mm - 1 ) * n + i ];
        for( j = 1; j < m - 1; ++j )
        {
            unsigned jj = 2 * j;
            realType c0 = J[ ( jj - 1 ) * n + i ] + ( q[ jj ] - q[ jj - 1 ] ) * D[ ( jj - 1 ) * n + i ];
            realType c1 = J[ ( jj + 1 ) * n + i ] + ( q[ jj ] - q[ jj + 1 ] ) * D[ ( jj + 1 ) * n + i ];
            realType c2 = J[ jj * n + i ];
            rminmax_3( &uv[ j ], &ov[ j ], c0, c1, c2 );
        }
    }
    free( q );
}

static void lob_bnd_ext_setup( lob_bnd_ext* p, const realType* z, const realType* w )
{
    unsigned i, mn = p->b.m * p->b.n;
    lob_bnd_base_setup( &p->b, z, w );
    for( i = 0; i < mn; ++i )
    {
        realType uvi = p->b.uv[ i ], ovi = p->b.ov[ i ];
        p->uvp[ i ] = p->uvn[ i ] = p->ovp[ i ] = p->ovn[ i ] = 0;
        if( uvi > 0 )
            p->uvp[ i ] = uvi;
        else
            p->uvn[ i ] = uvi;
        if( ovi > 0 )
            p->ovp[ i ] = ovi;
        else
            p->ovn[ i ] = ovi;
    }
}

static void lob_bnd_lines( const lob_bnd_base* p, const realType* u, realType* a, realType* b )
{
    unsigned        i, j;
    realType        a0 = 0, a1 = 0;
    const realType *uv = p->uv, *ov = p->ov;
    for( i = 0; i < p->n; ++i )
        a0 += p->Q0[ i ] * u[ i ], a1 += p->Q1[ i ] * u[ i ];
    for( j = 0; j < p->m; ++j )
        b[ j ] = a[ j ] = a0 + a1 * p->h[ j ];
    for( i = 0; i < p->n; ++i )
    {
        realType w = u[ i ] - ( a0 + a1 * p->z[ i ] );
        if( w >= 0 )
            for( j = 0; j < p->m; ++j )
                a[ j ] += w * ( *uv++ ), b[ j ] += w * ( *ov++ );
        else
            for( j = 0; j < p->m; ++j )
                a[ j ] += w * ( *ov++ ), b[ j ] += w * ( *uv++ );
    }
}

/* work holds p->m * 2 doubles */
static void lob_bnd_1( const lob_bnd_base* p, const realType* u, realType bnd[ 2 ], realType* work )
{
    unsigned  j;
    realType *a = work, *b = work + p->m;
    lob_bnd_lines( p, u, a, b );
    bnd[ 0 ] = a[ 0 ], bnd[ 1 ] = b[ 0 ];
    for( j = 1; j < p->m; ++j )
    {
        if( a[ j ] < bnd[ 0 ] ) bnd[ 0 ] = a[ j ];
        if( b[ j ] > bnd[ 1 ] ) bnd[ 1 ] = b[ j ];
    }
}

/* work holds 2*mr + 2*mr*ns + 2*mr*ms doubles */
static void lob_bnd_2( const lob_bnd_base* pr, const lob_bnd_ext* ps, const realType* u, realType bnd[ 2 ],
                       realType* work )
{
    unsigned  nr = pr->n, mr = pr->m, ns = ps->b.n, ms = ps->b.m;
    realType *a0 = work, *a1 = a0 + mr, *ar_ = a1 + mr, *ar = ar_, *br_ = ar + mr * ns, *br = br_, *a_ = br + mr * ns,
             *a = a_, *b_ = a + mr * ms, *b = b_, *uvp, *ovp, *uvn, *ovn;
    realType b0, b1;
    unsigned i, j, k;
    for( i = 0; i < mr; ++i )
        a0[ i ] = a1[ i ] = 0;
    for( j = 0; j < ns; ++j, u += nr )
    {
        realType q0 = ps->b.Q0[ j ], q1 = ps->b.Q1[ j ];
        lob_bnd_lines( pr, u, ar, br );
        for( i = 0; i < mr; ++i )
        {
            realType t = ( *ar++ + *br++ ) / 2;
            a0[ i ] += q0 * t, a1[ i ] += q1 * t;
        }
    }
    for( i = 0; i < mr; ++i )
    {
        realType a0i = a0[ i ], a1i = a1[ i ];
        for( k = 0; k < ms; ++k )
            *b++ = *a++ = a0i + a1i * ps->b.h[ k ];
    }
    ar = ar_, br = br_;
    uvp = ps->uvp, ovp = ps->ovp, uvn = ps->uvn, ovn = ps->ovn;
    for( j = 0; j < ns; ++j, uvp += ms, uvn += ms, ovp += ms, ovn += ms )
    {
        realType zj = ps->b.z[ j ];
        a = a_, b = b_;
        for( i = 0; i < mr; ++i )
        {
            realType t = a0[ i ] + a1[ i ] * zj;
            realType uw = *ar++ - t;
            realType ow = *br++ - t;
            if( uw >= 0 ) /* 0  <= uw <= ow */
                for( k = 0; k < ms; ++k )
                    *a++ += uw * uvp[ k ] + ow * uvn[ k ], *b++ += ow * ovp[ k ] + uw * ovn[ k ];
            else if( ow <= 0 ) /* uw <= ow <= 0  */
                for( k = 0; k < ms; ++k )
                    *a++ += uw * ovp[ k ] + ow * ovn[ k ], *b++ += ow * uvp[ k ] + uw * uvn[ k ];
            else /* uw <  0  <  ow */
                for( k = 0; k < ms; ++k )
                    *a++ += uw * ovp[ k ] + ow * uvn[ k ], *b++ += ow * ovp[ k ] + uw * uvn[ k ];
        }
    }
    b0 = a_[ 0 ], b1 = b_[ 0 ];
    for( i = 1; i < mr * ms; ++i )
    {
        if( a_[ i ] < b0 ) b0 = a_[ i ];
        if( b_[ i ] > b1 ) b1 = b_[ i ];
    }
    bnd[ 0 ] = b0, bnd[ 1 ] = b1;
}

/*--------------------------------------------------------------------------
   Small Matrix Inverse
  --------------------------------------------------------------------------*/

static void mat_inv_2( const realType A[ 4 ], realType inv[ 4 ] )
{
    const realType idet = 1 / ( A[ 0 ] * A[ 3 ] - A[ 1 ] * A[ 2 ] );
    inv[ 0 ] = idet * A[ 3 ];
    inv[ 1 ] = -( idet * A[ 1 ] );
    inv[ 2 ] = -( idet * A[ 2 ] );
    inv[ 3 ] = idet * A[ 0 ];
}

static void mat_inv_3( const realType A[ 9 ], realType inv[ 9 ] )
{
    const realType a = A[ 4 ] * A[ 8 ] - A[ 5 ] * A[ 7 ], b = A[ 5 ] * A[ 6 ] - A[ 3 ] * A[ 8 ],
                   c = A[ 3 ] * A[ 7 ] - A[ 4 ] * A[ 6 ], idet = 1 / ( A[ 0 ] * a + A[ 1 ] * b + A[ 2 ] * c );
    inv[ 0 ] = idet * a;
    inv[ 1 ] = idet * ( A[ 2 ] * A[ 7 ] - A[ 1 ] * A[ 8 ] );
    inv[ 2 ] = idet * ( A[ 1 ] * A[ 5 ] - A[ 2 ] * A[ 4 ] );
    inv[ 3 ] = idet * b;
    inv[ 4 ] = idet * ( A[ 0 ] * A[ 8 ] - A[ 2 ] * A[ 6 ] );
    inv[ 5 ] = idet * ( A[ 2 ] * A[ 3 ] - A[ 0 ] * A[ 5 ] );
    inv[ 6 ] = idet * c;
    inv[ 7 ] = idet * ( A[ 1 ] * A[ 6 ] - A[ 0 ] * A[ 7 ] );
    inv[ 8 ] = idet * ( A[ 0 ] * A[ 4 ] - A[ 1 ] * A[ 3 ] );
}

static void mat_app_2r( realType y[ 2 ], const realType A[ 4 ], const realType x[ 2 ] )
{
    y[ 0 ] = A[ 0 ] * x[ 0 ] + A[ 1 ] * x[ 1 ];
    y[ 1 ] = A[ 2 ] * x[ 0 ] + A[ 3 ] * x[ 1 ];
}

static void mat_app_2c( realType y[ 2 ], const realType A[ 4 ], const realType x[ 2 ] )
{
    y[ 0 ] = A[ 0 ] * x[ 0 ] + A[ 2 ] * x[ 1 ];
    y[ 1 ] = A[ 1 ] * x[ 0 ] + A[ 3 ] * x[ 1 ];
}

static void mat_app_3r( realType y[ 3 ], const realType A[ 9 ], const realType x[ 3 ] )
{
    y[ 0 ] = A[ 0 ] * x[ 0 ] + A[ 1 ] * x[ 1 ] + A[ 2 ] * x[ 2 ];
    y[ 1 ] = A[ 3 ] * x[ 0 ] + A[ 4 ] * x[ 1 ] + A[ 5 ] * x[ 2 ];
    y[ 2 ] = A[ 6 ] * x[ 0 ] + A[ 7 ] * x[ 1 ] + A[ 8 ] * x[ 2 ];
}

static void mat_app_3c( realType y[ 3 ], const realType A[ 9 ], const realType x[ 3 ] )
{
    y[ 0 ] = A[ 0 ] * x[ 0 ] + A[ 3 ] * x[ 1 ] + A[ 6 ] * x[ 2 ];
    y[ 1 ] = A[ 1 ] * x[ 0 ] + A[ 4 ] * x[ 1 ] + A[ 7 ] * x[ 2 ];
    y[ 2 ] = A[ 2 ] * x[ 0 ] + A[ 5 ] * x[ 1 ] + A[ 8 ] * x[ 2 ];
}

static void tinyla_solve_2( realType x[ 2 ], const realType A[ 4 ], const realType b[ 2 ] )
{
    realType inv[ 4 ];
    mat_inv_2( A, inv );
    mat_app_2r( x, inv, b );
}

static void tinyla_solve_3( realType x[ 3 ], const realType A[ 9 ], const realType b[ 3 ] )
{
    realType inv[ 9 ];
    mat_inv_3( A, inv );
    mat_app_3r( x, inv, b );
}

/* solve
   A[0] x0 + A[2] x1 = b0,
   A[2] x0 + A[1] x1 = b1
*/
static void tinyla_solve_sym_2( realType* x0, realType* x1, const realType A[ 3 ], realType b0, realType b1 )
{
    const realType idet = 1 / ( A[ 0 ] * A[ 1 ] - A[ 2 ] * A[ 2 ] );
    *x0 = idet * ( A[ 1 ] * b0 - A[ 2 ] * b1 );
    *x1 = idet * ( A[ 0 ] * b1 - A[ 2 ] * b0 );
}

/*--------------------------------------------------------------------------
   Oriented Bounding Box

   Suffixes on names are _2 for 2-d and _3 for 3-d

   Needed inputs are the Gauss-Lobatto quadrature nodes and weights:
     unsigned nr = ..., ns = ...;
     realType zr[nr], wr[nr];
     realType zs[ns], ws[ns];

     lobatto_nodes(zr,nr); lobatto_weights(zr,wr,nr);
     lobatto_nodes(zs,ns); lobatto_weights(zs,ws,ns);

   The number of points in the constructed piecewise (bi-)linear bounds
   for the boundaries is a parameter; more points give tighter bounds

     unsigned mr = 2*nr, ms = 2*ns;

   Bounding boxes are increased by a relative amount as a parameter

     realType tol = 0.01;

   Setup is accomplished via:

     const realType *z[2] = {zr,zs}, *w[2] = {wr,ws};
     const unsigned n[2] = {nr,ns}, m[2] = {mr,ms};
     obbox_data_2 *data = obbox_setup_2(z,w,n,m);

   Bounding box data may then be computed:

     obbox_2 box;                 // will store bounding box information
     realType xm[ns][nr], ym[ns][nr]; // x, y coordinates of the element nodes

     obbox_calc_2(data, tol, (const realType *)&xm[0][0],
                             (const realType *)&ym[0][0], &box);

   A point may be tested:

     const realType x[2]; // point to test
     realType r[2];

     if( obbox_axis_test_2(&box, x) )
       ... // x failed axis-aligned bounding box test

     if( obbox_test_2(&box, x, r) )
       ... // x failed oriented bounding box test
     else
       ... // r suitable as initial guess for parametric coords

   Once all bounding box information has been computed

     obbox_free_2(data);

   to free the memory allocated with obbox_setup_2.

  --------------------------------------------------------------------------*/

typedef struct
{
    lob_bnd_base dr, ds;
    realType *   Jr0, *Dr0, *Js0, *Ds0, *work;
} obbox_data_2;

typedef struct
{
    lob_bnd_base dr;
    lob_bnd_ext  ds, dt;
    realType *   Jr0, *Dr0, *Js0, *Ds0, *Jt0, *Dt0, *work;
} obbox_data_3;

static void obbox_data_alloc_2( obbox_data_2* p, const unsigned n[ 2 ], const unsigned m[ 2 ] )
{
    const unsigned max_npm = umax_2( n[ 0 ] + m[ 0 ], n[ 1 ] + m[ 1 ] );
    lob_bnd_base_alloc( &p->dr, n[ 0 ], m[ 0 ] );
    lob_bnd_base_alloc( &p->ds, n[ 1 ], m[ 1 ] );
    p->Jr0 = tmalloc( realType, 2 * n[ 0 ] + 2 * n[ 1 ] + 2 * max_npm );
    p->Dr0 = p->Jr0 + n[ 0 ];
    p->Js0 = p->Dr0 + n[ 0 ];
    p->Ds0 = p->Js0 + n[ 1 ];
    p->work = p->Ds0 + n[ 1 ];
}

static void obbox_data_free_2( obbox_data_2* p )
{
    lob_bnd_base_free( &p->dr );
    lob_bnd_base_free( &p->ds );
    free( p->Jr0 );
}

static void obbox_data_alloc_3( obbox_data_3* p, const unsigned n[ 3 ], const unsigned m[ 3 ] )
{
    const unsigned wk1 = 3 * n[ 0 ] * n[ 1 ] + 2 * m[ 0 ] + 2 * m[ 0 ] * n[ 1 ] + 2 * m[ 0 ] * m[ 1 ];
    const unsigned wk2 = 3 * n[ 0 ] * n[ 2 ] + 2 * m[ 0 ] + 2 * m[ 0 ] * n[ 2 ] + 2 * m[ 0 ] * m[ 2 ];
    const unsigned wk3 = 3 * n[ 1 ] * n[ 2 ] + 2 * m[ 1 ] + 2 * m[ 1 ] * n[ 2 ] + 2 * m[ 1 ] * m[ 2 ];
    const unsigned wk_max = umax_3( wk1, wk2, wk3 );
    lob_bnd_base_alloc( &p->dr, n[ 0 ], m[ 0 ] );
    lob_bnd_ext_alloc( &p->ds, n[ 1 ], m[ 1 ] );
    lob_bnd_ext_alloc( &p->dt, n[ 2 ], m[ 2 ] );
    p->Jr0 = tmalloc( realType, 2 * n[ 0 ] + 2 * n[ 1 ] + 2 * n[ 2 ] + wk_max );
    p->Dr0 = p->Jr0 + n[ 0 ];
    p->Js0 = p->Dr0 + n[ 0 ];
    p->Ds0 = p->Js0 + n[ 1 ];
    p->Jt0 = p->Ds0 + n[ 1 ];
    p->Dt0 = p->Jt0 + n[ 2 ];
    p->work = p->Dt0 + n[ 2 ];
}

static void obbox_data_free_3( obbox_data_3* p )
{
    lob_bnd_base_free( &p->dr );
    lob_bnd_ext_free( &p->ds );
    lob_bnd_ext_free( &p->dt );
    free( p->Jr0 );
}

static obbox_data_2* obbox_setup_2( const realType* const z[ 2 ], const realType* const w[ 2 ], const unsigned n[ 2 ],
                                    const unsigned m[ 2 ] )
{
    const realType zero = 0;
    realType*      work;
    obbox_data_2*  p = tmalloc( obbox_data_2, 1 );
    obbox_data_alloc_2( p, n, m );
    lob_bnd_base_setup( &p->dr, z[ 0 ], w[ 0 ] );
    lob_bnd_base_setup( &p->ds, z[ 1 ], w[ 1 ] );
    work = tmalloc( realType, 6 * umax_2( n[ 0 ], n[ 1 ] ) );
    lagrange_weights_deriv( z[ 0 ], n[ 0 ], &zero, 1, p->Jr0, p->Dr0, work );
    lagrange_weights_deriv( z[ 1 ], n[ 1 ], &zero, 1, p->Js0, p->Ds0, work );
    free( work );
    return p;
}

static obbox_data_3* obbox_setup_3( const realType* const z[ 3 ], const realType* const w[ 3 ], const unsigned n[ 3 ],
                                    const unsigned m[ 3 ] )
{
    const realType zero = 0;
    realType*      work;
    obbox_data_3*  p = tmalloc( obbox_data_3, 1 );
    obbox_data_alloc_3( p, n, m );
    lob_bnd_base_setup( &p->dr, z[ 0 ], w[ 0 ] );
    lob_bnd_ext_setup( &p->ds, z[ 1 ], w[ 1 ] );
    lob_bnd_ext_setup( &p->dt, z[ 2 ], w[ 2 ] );
    work = tmalloc( realType, 6 * umax_3( n[ 0 ], n[ 1 ], n[ 2 ] ) );
    lagrange_weights_deriv( z[ 0 ], n[ 0 ], &zero, 1, p->Jr0, p->Dr0, work );
    lagrange_weights_deriv( z[ 1 ], n[ 1 ], &zero, 1, p->Js0, p->Ds0, work );
    lagrange_weights_deriv( z[ 2 ], n[ 2 ], &zero, 1, p->Jt0, p->Dt0, work );
    free( work );
    return p;
}

static void obbox_free_2( obbox_data_2* p )
{
    obbox_data_free_2( p );
    free( p );
}

static void obbox_free_3( obbox_data_3* p )
{
    obbox_data_free_3( p );
    free( p );
}

int obbox_axis_test_2( const obbox_2* p, const realType x[ 2 ] )
{
    return ( x[ 0 ] < p->axis_bnd[ 0 ] || x[ 0 ] > p->axis_bnd[ 1 ] || x[ 1 ] < p->axis_bnd[ 2 ] ||
             x[ 1 ] > p->axis_bnd[ 3 ] );
}

int obbox_axis_test_3( const obbox_3* p, const realType x[ 3 ] )
{
    return ( x[ 0 ] < p->axis_bnd[ 0 ] || x[ 0 ] > p->axis_bnd[ 1 ] || x[ 1 ] < p->axis_bnd[ 2 ] ||
             x[ 1 ] > p->axis_bnd[ 3 ] || x[ 2 ] < p->axis_bnd[ 4 ] || x[ 2 ] > p->axis_bnd[ 5 ] );
}

int obbox_test_2( const obbox_2* p, const realType x[ 2 ], realType r[ 2 ] )
{
    realType xt[ 2 ];
    xt[ 0 ] = x[ 0 ] - p->x[ 0 ];
    xt[ 1 ] = x[ 1 ] - p->x[ 1 ];
    r[ 0 ] = p->A[ 0 ] * xt[ 0 ] + p->A[ 1 ] * xt[ 1 ];
    if( mbabs( r[ 0 ] ) > 1 ) return 1;
    r[ 1 ] = p->A[ 2 ] * xt[ 0 ] + p->A[ 3 ] * xt[ 1 ];
    return mbabs( r[ 1 ] ) > 1;
}

int obbox_test_3( const obbox_3* p, const realType x[ 3 ], realType r[ 3 ] )
{
    realType xt[ 3 ];
    xt[ 0 ] = x[ 0 ] - p->x[ 0 ];
    xt[ 1 ] = x[ 1 ] - p->x[ 1 ];
    xt[ 2 ] = x[ 2 ] - p->x[ 2 ];
    r[ 0 ] = p->A[ 0 ] * xt[ 0 ] + p->A[ 1 ] * xt[ 1 ] + p->A[ 2 ] * xt[ 2 ];
    if( mbabs( r[ 0 ] ) > 1 ) return 1;
    r[ 1 ] = p->A[ 3 ] * xt[ 0 ] + p->A[ 4 ] * xt[ 1 ] + p->A[ 5 ] * xt[ 2 ];
    if( mbabs( r[ 1 ] ) > 1 ) return 1;
    r[ 2 ] = p->A[ 6 ] * xt[ 0 ] + p->A[ 7 ] * xt[ 1 ] + p->A[ 8 ] * xt[ 2 ];
    return mbabs( r[ 2 ] ) > 1;
}

void obbox_calc_tfm_2( const realType* x, const realType* y, unsigned n, unsigned s, const realType c0[ 2 ],
                       const realType A[ 4 ], realType* u )
{
    unsigned  i;
    realType* v = u + n;
    for( i = 0; i < n; ++i, x += s, y += s )
    {
        const realType xt = *x - c0[ 0 ], yt = *y - c0[ 1 ];
        u[ i ] = A[ 0 ] * xt + A[ 1 ] * yt;
        v[ i ] = A[ 2 ] * xt + A[ 3 ] * yt;
    }
}

void obbox_calc_tfm_3( const realType* x, const realType* y, const realType* z, unsigned nr, unsigned sr, unsigned ns,
                       unsigned ss, const realType c0[ 3 ], const realType A[ 9 ], realType* u )
{
    unsigned  i, j;
    realType *v = u + nr * ns, *w = v + nr * ns;
    for( j = 0; j < ns; ++j, x += ss, y += ss, z += ss )
    {
        for( i = 0; i < nr; ++i, x += sr, y += sr, z += sr )
        {
            const realType xt = *x - c0[ 0 ], yt = *y - c0[ 1 ], zt = *z - c0[ 2 ];
            *u++ = A[ 0 ] * xt + A[ 1 ] * yt + A[ 2 ] * zt;
            *v++ = A[ 3 ] * xt + A[ 4 ] * yt + A[ 5 ] * zt;
            *w++ = A[ 6 ] * xt + A[ 7 ] * yt + A[ 8 ] * zt;
        }
    }
}

void obbox_merge_2( realType* b, const realType* ob )
{
    if( ob[ 0 ] < b[ 0 ] ) b[ 0 ] = ob[ 0 ];
    if( ob[ 1 ] > b[ 1 ] ) b[ 1 ] = ob[ 1 ];
    if( ob[ 2 ] < b[ 2 ] ) b[ 2 ] = ob[ 2 ];
    if( ob[ 3 ] > b[ 3 ] ) b[ 3 ] = ob[ 3 ];
}

void obbox_merge_3( realType* b, const realType* ob )
{
    if( ob[ 0 ] < b[ 0 ] ) b[ 0 ] = ob[ 0 ];
    if( ob[ 1 ] > b[ 1 ] ) b[ 1 ] = ob[ 1 ];
    if( ob[ 2 ] < b[ 2 ] ) b[ 2 ] = ob[ 2 ];
    if( ob[ 3 ] > b[ 3 ] ) b[ 3 ] = ob[ 3 ];
    if( ob[ 4 ] < b[ 4 ] ) b[ 4 ] = ob[ 4 ];
    if( ob[ 5 ] > b[ 5 ] ) b[ 5 ] = ob[ 5 ];
}

/* work holds 2*n + 2*m realTypes */
void obbox_side_2( const realType* x, const realType* y, unsigned n, unsigned s, const realType c0[ 2 ],
                   const realType A[ 4 ], realType* work, const lob_bnd_base* lbd, realType bnd[ 4 ] )
{
    obbox_calc_tfm_2( x, y, n, s, c0, A, work );
    lob_bnd_1( lbd, work, bnd, work + 2 * n );
    lob_bnd_1( lbd, work + n, bnd + 2, work + 2 * n );
}

/* work holds 3*nr*ns + 2*mr + 2*mr*ns + 2*mr*ms realTypes */
void obbox_side_3( const realType* x, const realType* y, const realType* z, unsigned nr, unsigned sr, unsigned ns,
                   unsigned ss, const realType c0[ 3 ], const realType A[ 9 ], realType* work, const lob_bnd_base* dr,
                   const lob_bnd_ext* ds, realType bnd[ 6 ] )
{
    obbox_calc_tfm_3( x, y, z, nr, sr, ns, ss, c0, A, work );
    lob_bnd_2( dr, ds, work, bnd, work + 3 * nr * ns );
    lob_bnd_2( dr, ds, work + nr * ns, bnd + 2, work + 3 * nr * ns );
    lob_bnd_2( dr, ds, work + 2 * nr * ns, bnd + 4, work + 3 * nr * ns );
}

/* return bounds on u = A (x - c0)
   bnd[0] <= u_0 <= bnd[1]
   bnd[2] <= u_1 <= bnd[3] */
void obbox_bnd_2( const obbox_data_2* p, const realType* x, const realType* y, const realType c0[ 2 ],
                  const realType A[ 4 ], realType bnd[ 4 ] )
{
    unsigned i, nr = p->dr.n, ns = p->ds.n;
    realType obnd[ 4 ];

    i = nr * ( ns - 1 );
    obbox_side_2( x, y, nr, 1, c0, A, p->work, &p->dr, bnd );
    obbox_side_2( x + i, y + i, nr, 1, c0, A, p->work, &p->dr, obnd );
    obbox_merge_2( bnd, obnd );

    i = nr - 1;
    obbox_side_2( x, y, ns, nr, c0, A, p->work, &p->ds, obnd );
    obbox_merge_2( bnd, obnd );
    obbox_side_2( x + i, y + i, nr, nr, c0, A, p->work, &p->ds, obnd );
    obbox_merge_2( bnd, obnd );
}

/* return bounds on u = A (x - c0)
   bnd[0] <= u_0 <= bnd[1]
   bnd[2] <= u_1 <= bnd[3]
   bnd[4] <= u_2 <= bnd[5] */
void obbox_bnd_3( const obbox_data_3* p, const realType* x, const realType* y, const realType* z,
                  const realType c0[ 3 ], const realType A[ 9 ], realType bnd[ 6 ] )
{
    unsigned i, nr = p->dr.n, ns = p->ds.b.n, nt = p->dt.b.n;
    realType obnd[ 6 ];

    i = nr * ns * ( nt - 1 );
    obbox_side_3( x, y, z, nr, 1, ns, 0, c0, A, p->work, &p->dr, &p->ds, bnd );
    obbox_side_3( x + i, y + i, z + i, nr, 1, ns, 0, c0, A, p->work, &p->dr, &p->ds, obnd );
    obbox_merge_3( bnd, obnd );

    i = nr * ( ns - 1 );
    obbox_side_3( x, y, z, nr, 1, nt, i, c0, A, p->work, &p->dr, &p->dt, obnd );
    obbox_merge_3( bnd, obnd );
    obbox_side_3( x + i, y + i, z + i, nr, 1, nt, i, c0, A, p->work, &p->dr, &p->dt, obnd );
    obbox_merge_3( bnd, obnd );

    i = nr - 1;
    obbox_side_3( x, y, z, ns, nr, nt, 0, c0, A, p->work, &p->ds.b, &p->dt, obnd );
    obbox_merge_3( bnd, obnd );
    obbox_side_3( x + i, y + i, z + i, ns, nr, nt, 0, c0, A, p->work, &p->ds.b, &p->dt, obnd );
    obbox_merge_3( bnd, obnd );
}

void obbox_calc_2( const obbox_data_2* p, realType tol, const realType* x, const realType* y, obbox_2* b )
{
    const realType zero[ 2 ] = { 0, 0 }, id[ 4 ] = { 1, 0, 0, 1 };
    realType       c0[ 2 ], jac[ 4 ], inv[ 4 ], bnd[ 4 ], u0[ 2 ], d[ 2 ];

    obbox_bnd_2( p, x, y, zero, id, b->axis_bnd );
    d[ 0 ] = b->axis_bnd[ 1 ] - b->axis_bnd[ 0 ];
    d[ 1 ] = b->axis_bnd[ 3 ] - b->axis_bnd[ 2 ];
    b->axis_bnd[ 0 ] -= tol * d[ 0 ], b->axis_bnd[ 1 ] += tol * d[ 0 ];
    b->axis_bnd[ 2 ] -= tol * d[ 1 ], b->axis_bnd[ 3 ] += tol * d[ 1 ];

    c0[ 0 ] = tensor_ig2( p->Jr0, p->Dr0, p->dr.n, p->Js0, p->Ds0, p->ds.n, x, jac, p->work );
    c0[ 1 ] = tensor_ig2( p->Jr0, p->Dr0, p->dr.n, p->Js0, p->Ds0, p->ds.n, y, jac + 2, p->work );
    mat_inv_2( jac, inv );

    obbox_bnd_2( p, x, y, c0, inv, bnd );

    u0[ 0 ] = ( bnd[ 0 ] + bnd[ 1 ] ) / 2;
    u0[ 1 ] = ( bnd[ 2 ] + bnd[ 3 ] ) / 2;
    d[ 0 ] = 2 / ( ( 1 + tol ) * ( bnd[ 1 ] - bnd[ 0 ] ) );
    d[ 1 ] = 2 / ( ( 1 + tol ) * ( bnd[ 3 ] - bnd[ 2 ] ) );
    b->x[ 0 ] = c0[ 0 ] + jac[ 0 ] * u0[ 0 ] + jac[ 1 ] * u0[ 1 ];
    b->x[ 1 ] = c0[ 1 ] + jac[ 2 ] * u0[ 0 ] + jac[ 3 ] * u0[ 1 ];
    b->A[ 0 ] = d[ 0 ] * inv[ 0 ], b->A[ 1 ] = d[ 0 ] * inv[ 1 ];
    b->A[ 2 ] = d[ 1 ] * inv[ 2 ], b->A[ 3 ] = d[ 1 ] * inv[ 3 ];
}

void obbox_calc_3( const obbox_data_3* p, realType tol, const realType* x, const realType* y, const realType* z,
                   obbox_3* b )
{
    const realType zero[ 3 ] = { 0, 0 }, id[ 9 ] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
    realType       c0[ 3 ], jac[ 9 ], inv[ 9 ], bnd[ 6 ], u0[ 3 ], d[ 3 ];

    obbox_bnd_3( p, x, y, z, zero, id, b->axis_bnd );
    d[ 0 ] = b->axis_bnd[ 1 ] - b->axis_bnd[ 0 ];
    d[ 1 ] = b->axis_bnd[ 3 ] - b->axis_bnd[ 2 ];
    d[ 2 ] = b->axis_bnd[ 5 ] - b->axis_bnd[ 4 ];
    b->axis_bnd[ 0 ] -= tol * d[ 0 ], b->axis_bnd[ 1 ] += tol * d[ 0 ];
    b->axis_bnd[ 2 ] -= tol * d[ 1 ], b->axis_bnd[ 3 ] += tol * d[ 1 ];
    b->axis_bnd[ 4 ] -= tol * d[ 2 ], b->axis_bnd[ 5 ] += tol * d[ 2 ];

    c0[ 0 ] =
        tensor_ig3( p->Jr0, p->Dr0, p->dr.n, p->Js0, p->Ds0, p->ds.b.n, p->Jt0, p->Dt0, p->dt.b.n, x, jac, p->work );
    c0[ 1 ] = tensor_ig3( p->Jr0, p->Dr0, p->dr.n, p->Js0, p->Ds0, p->ds.b.n, p->Jt0, p->Dt0, p->dt.b.n, y, jac + 3,
                          p->work );
    c0[ 2 ] = tensor_ig3( p->Jr0, p->Dr0, p->dr.n, p->Js0, p->Ds0, p->ds.b.n, p->Jt0, p->Dt0, p->dt.b.n, z, jac + 6,
                          p->work );
    mat_inv_3( jac, inv );

    obbox_bnd_3( p, x, y, z, c0, inv, bnd );

    u0[ 0 ] = ( bnd[ 0 ] + bnd[ 1 ] ) / 2;
    u0[ 1 ] = ( bnd[ 2 ] + bnd[ 3 ] ) / 2;
    u0[ 2 ] = ( bnd[ 4 ] + bnd[ 5 ] ) / 2;
    d[ 0 ] = 2 / ( ( 1 + tol ) * ( bnd[ 1 ] - bnd[ 0 ] ) );
    d[ 1 ] = 2 / ( ( 1 + tol ) * ( bnd[ 3 ] - bnd[ 2 ] ) );
    d[ 2 ] = 2 / ( ( 1 + tol ) * ( bnd[ 5 ] - bnd[ 4 ] ) );
    b->x[ 0 ] = c0[ 0 ] + jac[ 0 ] * u0[ 0 ] + jac[ 1 ] * u0[ 1 ] + jac[ 2 ] * u0[ 2 ];
    b->x[ 1 ] = c0[ 1 ] + jac[ 3 ] * u0[ 0 ] + jac[ 4 ] * u0[ 1 ] + jac[ 5 ] * u0[ 2 ];
    b->x[ 2 ] = c0[ 2 ] + jac[ 6 ] * u0[ 0 ] + jac[ 7 ] * u0[ 1 ] + jac[ 8 ] * u0[ 2 ];
    b->A[ 0 ] = d[ 0 ] * inv[ 0 ], b->A[ 1 ] = d[ 0 ] * inv[ 1 ], b->A[ 2 ] = d[ 0 ] * inv[ 2 ];
    b->A[ 3 ] = d[ 1 ] * inv[ 3 ], b->A[ 4 ] = d[ 1 ] * inv[ 4 ], b->A[ 5 ] = d[ 1 ] * inv[ 5 ];
    b->A[ 6 ] = d[ 2 ] * inv[ 6 ], b->A[ 7 ] = d[ 2 ] * inv[ 7 ], b->A[ 8 ] = d[ 2 ] * inv[ 8 ];
}

/*--------------------------------------------------------------------------
   Point to Possible Elements Hashing

   Initializing the data:
     unsigned nel;        // number of elements
     const unsigned n[3]; // number of nodes in r, s, t directions
     const realType *xm[3];   // n[0]*n[1]*n[2]*nel x,y,z coordinates
     realType tol = 0.01;     // how far point is allowed to be outside element
                          //   relative to element size
     unsigned max_size = n[0]*n[1]*n[2]*nel; // maximum size of hash table

     hash_data_3 data;
     hash_build_3(&data, xm, n, nel, max_size, tol);

   Using the data:
     realType x[3];   // point to find

     unsigned index = hash_index_3(&data, x);
     unsigned i, b = data.offset[index], e = data.offset[index+1];

     // point may be in elements
     //   data.offset[b], data.offset[b+1], ... , data.offset[e-1]
     //
     // list has maximum size data.max (e.g., e-b <= data.max)

     for(i=b; i!=e; ++i) {
       unsigned el = data.offset[i];
       const obbox_3 *obb = &data.obb[el]; // bounding box data for element el
       ...
     }

   When done:
     hash_free_3(&data);

  --------------------------------------------------------------------------*/

static int ifloor( realType x )
{
    /*
    int y = x;
    return (double)y > x ? y-1 : y;
    */
    return mbfloor( x );
}

static int iceil( realType x )
{
    /*
    int y = x;
    return (double)y < x ? y+1 : y;
    */
    return mbceil( x );
}

static unsigned hash_index_helper( realType low, realType fac, unsigned n, realType x )
{
    const int i = ifloor( ( x - low ) * fac );
    if( i < 0 ) return 0;
    return umin_2( i, n - 1 );
}

static uint hash_index_2( const hash_data_2* p, const realType x[ 2 ] )
{
    const unsigned n = p->hash_n;
    return (uint)hash_index_helper( p->bnd[ 2 ], p->fac[ 1 ], n, x[ 1 ] ) * n +
           hash_index_helper( p->bnd[ 0 ], p->fac[ 0 ], n, x[ 0 ] );
}

static uint hash_index_3( const hash_data_3* p, const realType x[ 3 ] )
{
    const unsigned n = p->hash_n;
    return ( (uint)hash_index_helper( p->bnd[ 4 ], p->fac[ 2 ], n, x[ 2 ] ) * n +
             hash_index_helper( p->bnd[ 2 ], p->fac[ 1 ], n, x[ 1 ] ) ) *
               n +
           hash_index_helper( p->bnd[ 0 ], p->fac[ 0 ], n, x[ 0 ] );
}

static void hash_setfac_2( hash_data_2* p, unsigned n )
{
    p->hash_n = n;
    p->fac[ 0 ] = n / ( p->bnd[ 1 ] - p->bnd[ 0 ] );
    p->fac[ 1 ] = n / ( p->bnd[ 3 ] - p->bnd[ 2 ] );
}

static void hash_setfac_3( hash_data_3* p, unsigned n )
{
    p->hash_n = n;
    p->fac[ 0 ] = n / ( p->bnd[ 1 ] - p->bnd[ 0 ] );
    p->fac[ 1 ] = n / ( p->bnd[ 3 ] - p->bnd[ 2 ] );
    p->fac[ 2 ] = n / ( p->bnd[ 5 ] - p->bnd[ 4 ] );
}

static void hash_range_2( const hash_data_2* p, uint i, unsigned d, unsigned* ia, unsigned* ib )
{
    const realType a = p->obb[ i ].axis_bnd[ d * 2 ];
    const realType b = p->obb[ i ].axis_bnd[ d * 2 + 1 ];
    const int      i0 = ifloor( ( a - p->bnd[ d * 2 ] ) * p->fac[ d ] );
    const unsigned i1 = iceil( ( b - p->bnd[ d * 2 ] ) * p->fac[ d ] );
    *ia = imax_2( 0, i0 );
    *ib = imin_2( i1, p->hash_n );
    if( *ib == *ia ) ++( *ib );
}

static void hash_range_3( const hash_data_3* p, uint i, unsigned d, unsigned* ia, unsigned* ib )
{
    const realType a = p->obb[ i ].axis_bnd[ d * 2 ];
    const realType b = p->obb[ i ].axis_bnd[ d * 2 + 1 ];
    const int      i0 = ifloor( ( a - p->bnd[ d * 2 ] ) * p->fac[ d ] );
    const unsigned i1 = iceil( ( b - p->bnd[ d * 2 ] ) * p->fac[ d ] );
    *ia = imax_2( 0, i0 );
    *ib = imin_2( i1, p->hash_n );
    if( *ib == *ia ) ++( *ib );
}

static uint hash_count_2( hash_data_2* p, uint nel, unsigned n )
{
    uint i, count = 0;
    hash_setfac_2( p, n );
    for( i = 0; i < nel; ++i )
    {
        unsigned ia, ib;
        uint     ci;
        hash_range_2( p, i, 0, &ia, &ib );
        ci = ib - ia;
        hash_range_2( p, i, 1, &ia, &ib );
        ci *= ib - ia;
        count += ci;
    }
    return count;
}

static uint hash_count_3( hash_data_3* p, uint nel, unsigned n )
{
    uint i, count = 0;
    hash_setfac_3( p, n );
    for( i = 0; i < nel; ++i )
    {
        unsigned ia, ib;
        uint     ci;
        hash_range_3( p, i, 0, &ia, &ib );
        ci = ib - ia;
        hash_range_3( p, i, 1, &ia, &ib );
        ci *= ib - ia;
        hash_range_3( p, i, 2, &ia, &ib );
        ci *= ib - ia;
        count += ci;
    }
    return count;
}

static uint hash_opt_size_2( hash_data_2* p, uint nel, uint max_size )
{
    unsigned nl = 1, nu = ceil( sqrt( max_size - nel ) );
    uint     size_low = 2 + nel;
    while( nu - nl > 1 )
    {
        unsigned nm = nl + ( nu - nl ) / 2;
        uint     size = (uint)nm * nm + 1 + hash_count_2( p, nel, nm );
        if( size <= max_size )
            nl = nm, size_low = size;
        else
            nu = nm;
    }
    hash_setfac_2( p, nl );
    return size_low;
}

static uint hash_opt_size_3( hash_data_3* p, uint nel, uint max_size )
{
    unsigned nl = 1, nu = ceil( pow( max_size - nel, 1.0 / 3 ) );
    uint     size_low = 2 + nel;
    while( nu - nl > 1 )
    {
        unsigned nm = nl + ( nu - nl ) / 2;
        uint     size = (uint)nm * nm * nm + 1 + hash_count_3( p, nel, nm );
        if( size <= max_size )
            nl = nm, size_low = size;
        else
            nu = nm;
    }
    hash_setfac_3( p, nl );
    return size_low;
}

static void hash_getbb_2( hash_data_2* p, const realType* const elx[ 2 ], const unsigned n[ 2 ], uint nel,
                          realType tol )
{
    obbox_data_2*   data;
    realType *      z[ 2 ], *w[ 2 ];
    uint            i;
    unsigned        d;
    const unsigned  nn = n[ 0 ] * n[ 1 ];
    unsigned int    m[ 2 ];
    const realType* x[ 2 ];

    for( i = 0; i < 2; ++i )
    {
        x[ i ] = elx[ i ];
        m[ i ] = 2 * n[ i ];
    }

    z[ 0 ] = tmalloc( realType, 2 * ( n[ 0 ] + n[ 1 ] ) );
    w[ 0 ] = z[ 0 ] + n[ 0 ];
    z[ 1 ] = w[ 0 ] + n[ 0 ], w[ 1 ] = z[ 1 ] + n[ 1 ];
    for( d = 0; d < 2; ++d )
        lobatto_nodes( z[ d ], n[ d ] ), lobatto_weights( z[ d ], w[ d ], n[ d ] );
    data = obbox_setup_2( (const realType* const*)z, (const realType* const*)w, n, m );
    obbox_calc_2( data, tol, x[ 0 ], x[ 1 ], &p->obb[ 0 ] );
    memcpy( &p->bnd[ 0 ], (const realType*)&p->obb[ 0 ].axis_bnd[ 0 ], 4 * sizeof( realType ) );
    for( i = 0; i < nel; ++i, x[ 0 ] += nn, x[ 1 ] += nn )
    {
        obbox_calc_2( data, tol, x[ 0 ], x[ 1 ], &p->obb[ i ] );
        obbox_merge_2( &p->bnd[ 0 ], (const realType*)&p->obb[ i ].axis_bnd[ 0 ] );
    }
    obbox_free_2( data );
    free( z[ 0 ] );
}

static void hash_getbb_3( hash_data_3* p, const realType* const elx[ 3 ], const unsigned n[ 3 ], uint nel,
                          realType tol )
{
    obbox_data_3*   data;
    const realType* x[ 3 ];
    realType *      z[ 3 ], *w[ 3 ];
    uint            i;
    unsigned        d;
    const unsigned  nn = n[ 0 ] * n[ 1 ] * n[ 2 ];
    unsigned int    m[ 3 ];

    for( i = 0; i < 3; ++i )
    {
        x[ i ] = elx[ i ];
        m[ i ] = 2 * n[ i ];
    }

    z[ 0 ] = tmalloc( realType, 2 * ( n[ 0 ] + n[ 1 ] + n[ 2 ] ) );
    w[ 0 ] = z[ 0 ] + n[ 0 ];
    for( d = 1; d < 3; ++d )
        z[ d ] = w[ d - 1 ] + n[ d - 1 ], w[ d ] = z[ d ] + n[ d ];
    for( d = 0; d < 3; ++d )
        lobatto_nodes( z[ d ], n[ d ] ), lobatto_weights( z[ d ], w[ d ], n[ d ] );
    data = obbox_setup_3( (const realType* const*)z, (const realType* const*)w, n, m );
    obbox_calc_3( data, tol, x[ 0 ], x[ 1 ], x[ 2 ], &p->obb[ 0 ] );
    memcpy( &p->bnd[ 0 ], (const realType*)&p->obb[ 0 ].axis_bnd[ 0 ], 6 * sizeof( realType ) );
    for( i = 0; i < nel; ++i, x[ 0 ] += nn, x[ 1 ] += nn, x[ 2 ] += nn )
    {
        obbox_calc_3( data, tol, x[ 0 ], x[ 1 ], x[ 2 ], &p->obb[ i ] );
        obbox_merge_3( &p->bnd[ 0 ], (const realType*)&p->obb[ i ].axis_bnd[ 0 ] );
    }
    obbox_free_3( data );
    free( z[ 0 ] );
}

static void hash_build_2( hash_data_2* p, const realType* const x[ 2 ], const unsigned n[ 2 ], uint nel,
                          uint max_hash_size, realType tol )
{
    uint      i, el, size, hn2, sum;
    unsigned  hn;
    unsigned* count;
    p->obb = tmalloc( obbox_2, nel );
    hash_getbb_2( p, x, n, nel, tol );
    size = hash_opt_size_2( p, nel, max_hash_size );
    p->offset = tmalloc( uint, size );
    hn = p->hash_n;
    hn2 = (uint)hn * hn;
    count = tcalloc( unsigned, hn2 );
    for( el = 0; el < nel; ++el )
    {
        unsigned ia, ib, j, ja, jb;
        hash_range_2( p, el, 0, &ia, &ib );
        hash_range_2( p, el, 1, &ja, &jb );
        for( j = ja; j < jb; ++j )
            for( i = ia; i < ib; ++i )
                ++count[ (uint)j * hn + i ];
    }
    sum = hn2 + 1, p->max = count[ 0 ];
    p->offset[ 0 ] = sum;
    for( i = 0; i < hn2; ++i )
    {
        if( count[ i ] > p->max ) p->max = count[ i ];
        sum += count[ i ];
        p->offset[ i + 1 ] = sum;
    }
    for( el = 0; el < nel; ++el )
    {
        unsigned ia, ib, j, ja, jb;
        hash_range_2( p, el, 0, &ia, &ib );
        hash_range_2( p, el, 1, &ja, &jb );
        for( j = ja; j < jb; ++j )
            for( i = ia; i < ib; ++i )
            {
                uint arrindex = (uint)j * hn + i;
                p->offset[ p->offset[ arrindex + 1 ] - count[ arrindex ] ] = el;
                --count[ arrindex ];
            }
    }
    free( count );
}

static void hash_build_3( hash_data_3* p, const realType* const x[ 3 ], const unsigned n[ 3 ], uint nel,
                          uint max_hash_size, realType tol )
{
    uint      i, el, size, hn3, sum;
    unsigned  hn;
    unsigned* count;
    p->obb = tmalloc( obbox_3, nel );
    hash_getbb_3( p, x, n, nel, tol );
    size = hash_opt_size_3( p, nel, max_hash_size );
    p->offset = tmalloc( uint, size );
    hn = p->hash_n;
    hn3 = (uint)hn * hn * hn;
    count = tcalloc( unsigned, hn3 );
    for( el = 0; el < nel; ++el )
    {
        unsigned ia, ib, j, ja, jb, k, ka, kb;
        hash_range_3( p, el, 0, &ia, &ib );
        hash_range_3( p, el, 1, &ja, &jb );
        hash_range_3( p, el, 2, &ka, &kb );
        for( k = ka; k < kb; ++k )
            for( j = ja; j < jb; ++j )
                for( i = ia; i < ib; ++i )
                    ++count[ ( (uint)k * hn + j ) * hn + i ];
    }
    sum = hn3 + 1, p->max = count[ 0 ];
    p->offset[ 0 ] = sum;
    for( i = 0; i < hn3; ++i )
    {
        if( count[ i ] > p->max ) p->max = count[ i ];
        sum += count[ i ];
        p->offset[ i + 1 ] = sum;
    }
    for( el = 0; el < nel; ++el )
    {
        unsigned ia, ib, j, ja, jb, k, ka, kb;
        hash_range_3( p, el, 0, &ia, &ib );
        hash_range_3( p, el, 1, &ja, &jb );
        hash_range_3( p, el, 2, &ka, &kb );
        for( k = ka; k < kb; ++k )
            for( j = ja; j < jb; ++j )
                for( i = ia; i < ib; ++i )
                {
                    uint arrindex = ( (uint)k * hn + j ) * hn + i;
                    p->offset[ p->offset[ arrindex + 1 ] - count[ arrindex ] ] = el;
                    --count[ arrindex ];
                }
    }
    free( count );
}

static void hash_free_2( hash_data_2* p )
{
    free( p->obb );
    free( p->offset );
}

static void hash_free_3( hash_data_3* p )
{
    free( p->obb );
    free( p->offset );
}

/*--------------------------------------------------------------------------
   Optimization algorithm to find a point within an element

   Given x(r)  (as values of x,y,z at all Lobatto nodes) and x_star,
   find the r that minimizes || x_star - x(r) ||_2

   As a minimization problem, the Newton step is

               __ 3
     [ J^T J - >_ d=1  resid_d H_d ] dr = J^t resid

   where resid = x_star - x(r), J = [ dx_i/dr_j ],
   and H_d = [ d^2 x_d/dr_i dr_j ].

   This is the appropriate step to take whenever constraints are active,
   and the current iterate is on a boundary of the element. When the current
   iterate is inside, J is square ( dim r = dim x ), resid will become small,
   and the step

     J dr = resid

   may be used instead, still giving quadratic convergence.


   Names use a _3 suffix for 3-d and _2 for 2-d.
   The routines require an initialized lagrange_data array as input:
     unsigned d, n[3] = { ... };
     realType *z[3] = { tmalloc(realType, n[0]), ... };
     for(d=0;d<3;++d) lobatto_nodes(z[d],n[d]);

     lagrange_data ld[3];
     for(d=0;d<3;++d) lagrange_setup(&ld[d],z[d],n[d]);

   Initialization:
     opt_data_3 data;
     opt_alloc_3(&data, ld);

   Use:
     const realType *xm[3];  // 3 pointers, each to n[0]*n[1]*n[2] realTypes
                         //   giving the nodal x, y, or z coordinates

     const realType x_star[3] = { ... };  // point to find
     realType r[3] = { 0,0,0 };           // initial guess with
     unsigned c = opt_no_constraints_3;   //   these constraints active

     realType dist = opt_findpt_3(&data,xm,x_star,r,&c);
     // minimizer is r with constraints c; 2-norm of resid is dist

   Clean-up:
     opt_free_3(&data);

     for(d=0;d<3;++d) lagrange_free(&ld[d]);
     for(d=0;d<3;++d) free(z[d]);

   The constraint number works as follows. Let cr be the constraints
   on the r variable:
      cr = 0       r fixed at -1
      cr = 1       r not fixed
      cr = 2       r fixed at 1
   Then the constraint number is (ct*3+cs)*3+cr

  --------------------------------------------------------------------------*/

/* how many directions are constrained? */
static const char opt_constr_num_2[ 9 ] = { 2, 1, 2, 1, 0, 1, 2, 1, 2 };
static const char opt_constr_num_3[ 27 ] = { 3, 2, 3, 2, 1, 2, 3, 2, 3, 2, 1, 2, 1, 0,
                                             1, 2, 1, 2, 3, 2, 3, 2, 1, 2, 3, 2, 3 };

/* which direction is constrained? */
/* static const char opt_constr_dir_2[9] = {-1, 1,-1,  0,-1, 0, -1, 1,-1}; */
static const char opt_constr_dir_3[ 27 ] = { -1, -1, -1, -1, 2,  -1, -1, -1, -1, -1, 1,  -1, 0, -1,
                                             0,  -1, 1,  -1, -1, -1, -1, -1, 2,  -1, -1, -1, -1 };

/* which direction is not constrained? */
static const char opt_constr_not[ 27 ] = { -1, 0, -1, 1, -1, 1, -1, 0, -1, 2, -1, 2, -1, -1,
                                           -1, 2, -1, 2, -1, 0, -1, 1, -1, 1, -1, 0, -1 };

static const char opt_constr_wide[ 27 ] = { 0x00, 0x01, 0x02, 0x04, 0x05, 0x06, 0x08, 0x09, 0x0a,
                                            0x10, 0x11, 0x12, 0x14, 0x15, 0x16, 0x18, 0x19, 0x1a,
                                            0x20, 0x21, 0x22, 0x24, 0x25, 0x26, 0x28, 0x29, 0x2a };

static const unsigned opt_other1_3[ 3 ] = { 1, 0, 0 }, opt_other2_3[ 3 ] = { 2, 2, 1 };

static unsigned opt_constr( unsigned constraints, unsigned d )
{
    return ( opt_constr_wide[ constraints ] >> ( d * 2 ) ) & 3;
}

static void opt_constr_unpack_2( unsigned constraints, unsigned* c )
{
    const char cw = opt_constr_wide[ constraints ];
    c[ 0 ] = cw & 3;
    c[ 1 ] = cw >> 2;
}

static void opt_constr_unpack_3( unsigned constraints, unsigned* c )
{
    const char cw = opt_constr_wide[ constraints ];
    c[ 0 ] = cw & 3;
    c[ 1 ] = ( cw >> 2 ) & 3;
    c[ 2 ] = cw >> 4;
}

static unsigned opt_constr_pack_2( const unsigned* c )
{
    return c[ 1 ] * 3 + c[ 0 ];
}

static unsigned opt_constr_pack_3( const unsigned* c )
{
    return ( c[ 2 ] * 3 + c[ 1 ] ) * 3 + c[ 0 ];
}

/*--------------------------------------------------------------------------

   3 - D

  --------------------------------------------------------------------------*/

void opt_alloc_3( opt_data_3* p, lagrange_data* ld )
{
    const unsigned nr = ld[ 0 ].n, ns = ld[ 1 ].n, nt = ld[ 2 ].n, nf = umax_3( nr * ns, nr * nt, ns * nt ),
                   ne = umax_3( nr, ns, nt ), nw = 2 * ns * nt + 3 * ns;
    p->size[ 0 ] = 1;
    p->size[ 1 ] = nr;
    p->size[ 2 ] = nr * ns;
    p->size[ 3 ] = p->size[ 2 ] * nt;
    p->ld = ld;
    p->work = tmalloc( realType, 6 * nf + 9 * ne + nw );
    p->fd.x[ 0 ] = p->work + nw;
    p->fd.x[ 1 ] = p->fd.x[ 0 ] + nf;
    p->fd.x[ 2 ] = p->fd.x[ 1 ] + nf;
    p->fd.fdn[ 0 ] = p->fd.x[ 2 ] + nf;
    p->fd.fdn[ 1 ] = p->fd.fdn[ 0 ] + nf;
    p->fd.fdn[ 2 ] = p->fd.fdn[ 1 ] + nf;
    p->ed.x[ 0 ] = p->fd.fdn[ 2 ] + nf;
    p->ed.x[ 1 ] = p->ed.x[ 0 ] + ne;
    p->ed.x[ 2 ] = p->ed.x[ 1 ] + ne;
    p->ed.fd1[ 0 ] = p->ed.x[ 2 ] + ne;
    p->ed.fd1[ 1 ] = p->ed.fd1[ 0 ] + ne;
    p->ed.fd1[ 2 ] = p->ed.fd1[ 1 ] + ne;
    p->ed.fd2[ 0 ] = p->ed.fd1[ 2 ] + ne;
    p->ed.fd2[ 1 ] = p->ed.fd2[ 0 ] + ne;
    p->ed.fd2[ 2 ] = p->ed.fd2[ 1 ] + ne;
}

void opt_free_3( opt_data_3* p )
{
    free( p->work );
}

static void opt_vol_set_3( opt_data_3* p, const realType r[ 3 ] )
{
    lagrange_1( &p->ld[ 0 ], r[ 0 ] );
    lagrange_1( &p->ld[ 1 ], r[ 1 ] );
    lagrange_1( &p->ld[ 2 ], r[ 2 ] );
}

/* work holds 2*ns*nt + 3*ns realTypes */
static void opt_vol_intp_3( opt_data_3* p )
{
    unsigned             d;
    const lagrange_data* ld = p->ld;

    for( d = 0; d < 3; ++d )
        p->x[ d ] = tensor_ig3( ld[ 0 ].J, ld[ 0 ].D, ld[ 0 ].n, ld[ 1 ].J, ld[ 1 ].D, ld[ 1 ].n, ld[ 2 ].J, ld[ 2 ].D,
                                ld[ 2 ].n, p->elx[ d ], &p->jac[ d * 3 ], p->work );
}

void opt_vol_set_intp_3( opt_data_3* p, const realType r[ 3 ] )
{
    opt_vol_set_3( p, r );
    opt_vol_intp_3( p );
}

static void opt_face_proj_3( opt_data_3* p )
{
    unsigned       d, off = 0;
    const unsigned dn = p->fd.dn, d1 = p->fd.d1, d2 = p->fd.d2, so = p->size[ d2 ] - p->size[ d1 + 1 ],
                   s1 = p->size[ d1 ], sn = p->size[ dn ], n1 = p->ld[ d1 ].n, n2 = p->ld[ d2 ].n, nn = p->ld[ dn ].n;
    const realType* D = p->ld[ dn ].D_z0;
    if( opt_constr( p->fd.constraints, dn ) == 2 ) off = p->size[ dn + 1 ] - p->size[ dn ], D = p->ld[ dn ].D_zn;
    for( d = 0; d < 3; ++d )
    {
        unsigned        i, j, k, arrindex = 0;
        const realType* in = p->elx[ d ] + off;
        for( j = n2; j; --j, in += so )
            for( i = n1; i; --i, ++arrindex, in += s1 )
            {
                const realType* ind = in - off;
                realType*       fdn = &p->fd.fdn[ d ][ arrindex ];
                p->fd.x[ d ][ arrindex ] = *in;
                *fdn = 0;
                for( k = 0; k < nn; ++k, ind += sn )
                    *fdn += *ind * D[ k ];
            }
    }
}

static void opt_face_set_3( opt_data_3* p, const realType r[ 3 ], unsigned constr )
{
    if( p->fd.constraints != constr )
    {
        p->fd.constraints = constr;
        p->fd.dn = opt_constr_dir_3[ constr ];
        p->fd.d1 = opt_other1_3[ p->fd.dn ];
        p->fd.d2 = opt_other2_3[ p->fd.dn ];
        opt_face_proj_3( p );
    }
    lagrange_1( &p->ld[ p->fd.d1 ], r[ p->fd.d1 ] );
    lagrange_1( &p->ld[ p->fd.d2 ], r[ p->fd.d2 ] );
}

/* work holds 2*ld[d2].n realTypes */
static void opt_face_intp_3( opt_data_3* p )
{
    unsigned        d;
    const unsigned  dn = p->fd.dn, d1 = p->fd.d1, d2 = p->fd.d2, n1 = p->ld[ d1 ].n, n2 = p->ld[ d2 ].n;
    const realType *J1 = p->ld[ d1 ].J, *J2 = p->ld[ d2 ].J, *D1 = p->ld[ d1 ].D, *D2 = p->ld[ d2 ].D;

    for( d = 0; d < 3; ++d )
    {
        realType g[ 2 ];
        p->x[ d ] = tensor_ig2( J1, D1, n1, J2, D2, n2, p->fd.x[ d ], &g[ 0 ], p->work );
        p->jac[ d * 3 + d1 ] = g[ 0 ];
        p->jac[ d * 3 + d2 ] = g[ 1 ];
        p->jac[ d * 3 + dn ] = tensor_i2( J1, n1, J2, n2, p->fd.fdn[ d ], p->work );
    }
}

static void opt_face_set_intp_3( opt_data_3* p, const realType r[ 3 ], unsigned constr )
{
    opt_face_set_3( p, r, constr );
    opt_face_intp_3( p );
}

static void opt_face_hess_3( opt_data_3* p, realType hess[ 9 ] )
{
    unsigned        d;
    const unsigned  d1 = p->fd.d1, d2 = p->fd.d2, n1 = p->ld[ d1 ].n, n2 = p->ld[ d2 ].n;
    const realType *J1 = p->ld[ d1 ].J, *J2 = p->ld[ d2 ].J, *D1 = p->ld[ d1 ].D, *D2 = p->ld[ d2 ].D,
                   *S1 = p->ld[ d1 ].D2, *S2 = p->ld[ d2 ].D2;

    lagrange_2u( &p->ld[ d1 ] );
    lagrange_2u( &p->ld[ d2 ] );

    for( d = 0; d < 3; ++d )
    {
        (void)tensor_ig2( J1, S1, n1, J2, S2, n2, p->fd.x[ d ], hess + d * 3, p->work );
        hess[ d * 3 + 0 ] = tensor_i2( S1, n1, J2, n2, p->fd.x[ d ], p->work );
        hess[ d * 3 + 1 ] = tensor_i2( J1, n1, S2, n2, p->fd.x[ d ], p->work );
        hess[ d * 3 + 2 ] = tensor_i2( D1, n1, D2, n2, p->fd.x[ d ], p->work );
    }
}

static void opt_edge_proj_3( opt_data_3* p )
{
    unsigned       d, off, off1 = 0, off2 = 0;
    const unsigned de = p->ed.de, d1 = p->ed.d1, d2 = p->ed.d2, se = p->size[ de ], s1 = p->size[ d1 ],
                   s2 = p->size[ d2 ], ne = p->ld[ de ].n, n1 = p->ld[ d1 ].n, n2 = p->ld[ d2 ].n;
    const realType *fD1, *fD2;
    if( opt_constr( p->ed.constraints, d1 ) == 0 )
        fD1 = p->ld[ d1 ].D_z0;
    else
        fD1 = p->ld[ d1 ].D_zn, off1 = p->size[ d1 + 1 ] - p->size[ d1 ];
    if( opt_constr( p->ed.constraints, d2 ) == 0 )
        fD2 = p->ld[ d2 ].D_z0;
    else
        fD2 = p->ld[ d2 ].D_zn, off2 = p->size[ d2 + 1 ] - p->size[ d2 ];
    off = off1 + off2;
    for( d = 0; d < 3; ++d )
    {
        unsigned        i, j;
        const realType* in = p->elx[ d ] + off;
        for( i = 0; i < ne; ++i, in += se )
        {
            const realType *in1 = in - off1, *in2 = in - off2;
            realType *      fd1 = &p->ed.fd1[ d ][ i ], *fd2 = &p->ed.fd2[ d ][ i ];
            p->ed.x[ d ][ i ] = *in;
            *fd1 = *fd2 = 0;
            for( j = 0; j < n1; ++j, in1 += s1 )
                *fd1 += *in1 * fD1[ j ];
            for( j = 0; j < n2; ++j, in2 += s2 )
                *fd2 += *in2 * fD2[ j ];
        }
    }
}

static void opt_edge_set_3( opt_data_3* p, const realType r[ 3 ], unsigned constr )
{
    if( p->ed.constraints != constr )
    {
        p->ed.constraints = constr;
        p->ed.de = opt_constr_not[ constr ];
        p->ed.d1 = opt_other1_3[ p->ed.de ];
        p->ed.d2 = opt_other2_3[ p->ed.de ];
        opt_edge_proj_3( p );
    }
    lagrange_1( &p->ld[ p->ed.de ], r[ p->ed.de ] );
}

static void opt_edge_intp_3( opt_data_3* p )
{
    unsigned        d;
    const unsigned  de = p->ed.de, d1 = p->ed.d1, d2 = p->ed.d2, n = p->ld[ de ].n;
    const realType *J = p->ld[ de ].J, *D = p->ld[ de ].D;

    for( d = 0; d < 3; ++d )
    {
        p->x[ d ] = tensor_ig1( J, D, n, p->ed.x[ d ], &p->jac[ d * 3 + de ] );
        p->jac[ d * 3 + d1 ] = tensor_i1( J, n, p->ed.fd1[ d ] );
        p->jac[ d * 3 + d2 ] = tensor_i1( J, n, p->ed.fd2[ d ] );
    }
}

static void opt_edge_set_intp_3( opt_data_3* p, const realType r[ 3 ], unsigned constr )
{
    opt_edge_set_3( p, r, constr );
    opt_edge_intp_3( p );
}

static void opt_edge_hess_3( opt_data_3* p, realType hess[ 3 ] )
{
    unsigned        d;
    const unsigned  de = p->ed.de, n = p->ld[ de ].n;
    const realType* D2 = p->ld[ de ].D2;
    lagrange_2u( &p->ld[ de ] );
    for( d = 0; d < 3; ++d )
        hess[ d ] = tensor_i1( D2, n, p->ed.x[ d ] );
}

static void opt_point_proj_3( opt_data_3* p )
{
    unsigned        off[ 3 ], offt, d, c[ 3 ];
    const realType* fD[ 3 ];
    opt_constr_unpack_3( p->pd.constraints, c );
    for( d = 0; d < 3; ++d )
        if( c[ d ] == 0 )
            fD[ d ] = p->ld[ d ].D_z0, off[ d ] = 0;
        else
            fD[ d ] = p->ld[ d ].D_zn, off[ d ] = p->size[ d + 1 ] - p->size[ d ];
    offt = off[ 0 ] + off[ 1 ] + off[ 2 ];
    for( d = 0; d < 9; ++d )
        p->pd.jac[ d ] = 0;
    for( d = 0; d < 3; ++d )
    {
        unsigned i, j;
        p->pd.x[ d ] = p->elx[ d ][ offt ];
        for( i = 0; i < 3; ++i )
        {
            const realType* in = p->elx[ d ] + offt - off[ i ];
            for( j = 0; j < p->ld[ i ].n; ++j, in += p->size[ i ] )
                p->pd.jac[ d * 3 + i ] += *in * fD[ i ][ j ];
        }
    }
}

static void opt_point_set_3( opt_data_3* p, unsigned constr )
{
    if( p->pd.constraints != constr )
    {
        p->pd.constraints = constr;
        opt_point_proj_3( p );
    }
}

static void opt_point_intp_3( opt_data_3* p )
{
    memcpy( p->x, p->pd.x, 3 * sizeof( realType ) );
    memcpy( p->jac, p->pd.jac, 9 * sizeof( realType ) );
}

static void opt_point_set_intp_3( opt_data_3* p, unsigned constr )
{
    opt_point_set_3( p, constr );
    opt_point_intp_3( p );
}

#define DIAGNOSTICS 0

double opt_findpt_3( opt_data_3* p, const realType* const elx[ 3 ], const realType xstar[ 3 ], realType r[ 3 ],
                     unsigned* constr )
{
    realType dr[ 3 ], resid[ 3 ], steep[ 3 ];

    unsigned c = *constr, ac, d, cc[ 3 ], step = 0;

    p->elx[ 0 ] = elx[ 0 ], p->elx[ 1 ] = elx[ 1 ], p->elx[ 2 ] = elx[ 2 ];

    p->fd.constraints = opt_no_constraints_3;
    p->ed.constraints = opt_no_constraints_3;
    p->pd.constraints = opt_no_constraints_3;

#if DIAGNOSTICS
    printf( "opt_findpt: xstar = %g, %g, %g\n", xstar[ 0 ], xstar[ 1 ], xstar[ 2 ] );
#endif

    do
    {
        ++step;
        if( step == 50 ) /*fail("%s: opt_findpt_3 did not converge\n",__FILE__);*/
            return 1.e+30;
#if DIAGNOSTICS
        printf( "  iteration %u\n", step );
        printf( "    %d constraint(s) active\n", (int)opt_constr_num_3[ c ] );
#endif
        /* update face/edge/point data if necessary,
           and evaluate x(r) as well as the jacobian */
        switch( opt_constr_num_3[ c ] )
        {
            case 0:
                opt_vol_set_intp_3( p, r );
                break;
            case 1:
                opt_face_set_intp_3( p, r, c );
                break;
            case 2:
                opt_edge_set_intp_3( p, r, c );
                break;
            case 3:
                opt_point_set_intp_3( p, c );
                break;
        }
#if DIAGNOSTICS
        printf( "    r = %g, %g, %g\n", r[ 0 ], r[ 1 ], r[ 2 ] );
        printf( "    x = %g, %g, %g\n", p->x[ 0 ], p->x[ 1 ], p->x[ 2 ] );
#endif
        /* compute residual */
        for( d = 0; d < 3; ++d )
            resid[ d ] = xstar[ d ] - p->x[ d ];
#if DIAGNOSTICS
        printf( "    resid = %g, %g, %g\n", resid[ 0 ], resid[ 1 ], resid[ 2 ] );
        printf( "    2-norm = %g\n", r2norm_3( resid[ 0 ], resid[ 1 ], resid[ 2 ] ) );
#endif
        /* check constraints against steepest descent direction */
        ac = c;
        if( opt_constr_num_3[ c ] )
        {
            opt_constr_unpack_3( c, cc );
            mat_app_3c( steep, p->jac, resid ); /* steepest descent = J^T r */
#if DIAGNOSTICS
            printf( "    steepest descent = %g, %g, %g\n", steep[ 0 ], steep[ 1 ], steep[ 2 ] );
#endif
            for( d = 0; d < 3; ++d )
                if( ( cc[ d ] == 0 && steep[ d ] > 0 ) || ( cc[ d ] == 2 && steep[ d ] < 0 ) ) cc[ d ] = 1;
            ac = opt_constr_pack_3( cc );
        }
        /* update face/edge/point data if necessary */
        if( ac != c )
        {
            c = ac;
#if DIAGNOSTICS
            printf( "    relaxed to %d constraints\n", (int)opt_constr_num_3[ c ] );
#endif
            switch( opt_constr_num_3[ c ] )
            {
                case 1:
                    opt_face_set_3( p, r, c );
                    break;
                case 2:
                    opt_edge_set_3( p, r, c );
                    break;
                case 3:
                    opt_point_set_3( p, c );
                    break;
            }
        }
        /* compute Newton step */
        switch( opt_constr_num_3[ c ] )
        {
            case 0:
                tinyla_solve_3( dr, p->jac, resid );
                break;
            case 1: {
                const unsigned  dn = p->fd.dn, d1 = p->fd.d1, d2 = p->fd.d2;
                realType        A[ 4 ], H[ 9 ];
                const realType* J = p->jac;
                opt_face_hess_3( p, H );
                A[ 0 ] = J[ d1 ] * J[ d1 ] + J[ 3 + d1 ] * J[ 3 + d1 ] + J[ 6 + d1 ] * J[ 6 + d1 ];
                A[ 1 ] = J[ d2 ] * J[ d2 ] + J[ 3 + d2 ] * J[ 3 + d2 ] + J[ 6 + d2 ] * J[ 6 + d2 ];
                A[ 2 ] = J[ d1 ] * J[ d2 ] + J[ 3 + d1 ] * J[ 3 + d2 ] + J[ 6 + d1 ] * J[ 6 + d2 ];
                A[ 0 ] -= resid[ 0 ] * H[ 0 ] + resid[ 1 ] * H[ 3 ] + resid[ 2 ] * H[ 6 ];
                A[ 1 ] -= resid[ 0 ] * H[ 1 ] + resid[ 1 ] * H[ 4 ] + resid[ 2 ] * H[ 7 ];
                A[ 2 ] -= resid[ 0 ] * H[ 2 ] + resid[ 1 ] * H[ 5 ] + resid[ 2 ] * H[ 8 ];
                tinyla_solve_sym_2( &dr[ d1 ], &dr[ d2 ], A, steep[ d1 ], steep[ d2 ] );
                dr[ dn ] = 0;
            }
            break;
            case 2: {
                const unsigned  de = p->ed.de, d1 = p->ed.d1, d2 = p->ed.d2;
                realType        fac, H[ 3 ];
                const realType* J = p->jac + de;
                opt_edge_hess_3( p, H );
                fac = J[ 0 ] * J[ 0 ] + J[ 3 ] * J[ 3 ] + J[ 6 ] * J[ 6 ] -
                      ( resid[ 0 ] * H[ 0 ] + resid[ 1 ] * H[ 1 ] + resid[ 2 ] * H[ 2 ] );
                dr[ de ] = steep[ de ] / fac;
                dr[ d1 ] = 0, dr[ d2 ] = 0;
            }
            break;
            case 3:
                dr[ 0 ] = dr[ 1 ] = dr[ 2 ] = 0;
                break;
        }
#if DIAGNOSTICS
        printf( "    dr = %g, %g, %g\n", dr[ 0 ], dr[ 1 ], dr[ 2 ] );
#endif
        /* project new iteration onto [-1,1]^3 */
        opt_constr_unpack_3( c, cc );
        for( d = 0; d < 3; ++d )
        {
            if( cc[ d ] != 1 ) continue;
            r[ d ] += dr[ d ];
            if( r[ d ] <= -1 )
                dr[ d ] -= r[ d ] + 1, r[ d ] = -1, cc[ d ] = 0;
            else if( r[ d ] >= 1 )
                dr[ d ] -= r[ d ] - 1, r[ d ] = 1, cc[ d ] = 2;
        }
        c = opt_constr_pack_3( cc );
    } while( r1norm_3( dr[ 0 ], dr[ 1 ], dr[ 2 ] ) > 3 * MOAB_POLY_EPS );
    *constr = c;
#if 0
  printf("opt_findpt_3 converged in %u iterations\n", step);
#endif
    return r2norm_3( resid[ 0 ], resid[ 1 ], resid[ 2 ] );
}

#undef DIAGNOSTICS

void opt_alloc_2( opt_data_2* p, lagrange_data* ld )
{
    const unsigned nr = ld[ 0 ].n, ns = ld[ 1 ].n, ne = umax_2( nr, ns ), nw = 2 * ns;
    p->size[ 0 ] = 1;
    p->size[ 1 ] = nr;
    p->size[ 2 ] = nr * ns;
    p->ld = ld;
    p->work = tmalloc( realType, 4 * ne + nw );
    p->ed.x[ 0 ] = p->work + nw;
    p->ed.x[ 1 ] = p->ed.x[ 0 ] + ne;
    p->ed.fd1[ 0 ] = p->ed.x[ 1 ] + ne;
    p->ed.fd1[ 1 ] = p->ed.fd1[ 0 ] + ne;
}

void opt_free_2( opt_data_2* p )
{
    free( p->work );
}

static void opt_area_set_2( opt_data_2* p, const realType r[ 2 ] )
{
    lagrange_1( &p->ld[ 0 ], r[ 0 ] );
    lagrange_1( &p->ld[ 1 ], r[ 1 ] );
}

/* work holds 2*ns realTypes */
static void opt_area_intp_2( opt_data_2* p )
{
    unsigned             d;
    const lagrange_data* ld = p->ld;

    for( d = 0; d < 2; ++d )
        p->x[ d ] = tensor_ig2( ld[ 0 ].J, ld[ 0 ].D, ld[ 0 ].n, ld[ 1 ].J, ld[ 1 ].D, ld[ 1 ].n, p->elx[ d ],
                                &p->jac[ d * 2 ], p->work );
}

static void opt_area_set_intp_2( opt_data_2* p, const realType r[ 2 ] )
{
    opt_area_set_2( p, r );
    opt_area_intp_2( p );
}

static void opt_edge_proj_2( opt_data_2* p )
{
    unsigned       d, off = 0;
    const unsigned de = p->ed.de, d1 = p->ed.d1, se = p->size[ de ], s1 = p->size[ d1 ], ne = p->ld[ de ].n,
                   n1 = p->ld[ d1 ].n;
    const realType* fD1;
    if( opt_constr( p->ed.constraints, d1 ) == 0 )
        fD1 = p->ld[ d1 ].D_z0;
    else
        fD1 = p->ld[ d1 ].D_zn, off = p->size[ d1 + 1 ] - p->size[ d1 ];
    for( d = 0; d < 2; ++d )
    {
        unsigned        i, j;
        const realType* in = p->elx[ d ] + off;
        for( i = 0; i < ne; ++i, in += se )
        {
            const realType* in1 = in - off;
            realType*       fd1 = &p->ed.fd1[ d ][ i ];
            p->ed.x[ d ][ i ] = *in;
            *fd1 = 0;
            for( j = 0; j < n1; ++j, in1 += s1 )
                *fd1 += *in1 * fD1[ j ];
        }
    }
}

static void opt_edge_set_2( opt_data_2* p, const realType r[ 2 ], unsigned constr )
{
    if( p->ed.constraints != constr )
    {
        p->ed.constraints = constr;
        p->ed.de = opt_constr_not[ constr ];
        p->ed.d1 = 1 - p->ed.de;
        opt_edge_proj_2( p );
    }
    lagrange_1( &p->ld[ p->ed.de ], r[ p->ed.de ] );
}

static void opt_edge_intp_2( opt_data_2* p )
{
    unsigned        d;
    const unsigned  de = p->ed.de, d1 = p->ed.d1, n = p->ld[ de ].n;
    const realType *J = p->ld[ de ].J, *D = p->ld[ de ].D;
    for( d = 0; d < 2; ++d )
    {
        p->x[ d ] = tensor_ig1( J, D, n, p->ed.x[ d ], &p->jac[ d * 2 + de ] );
        p->jac[ d * 2 + d1 ] = tensor_i1( J, n, p->ed.fd1[ d ] );
    }
}

static void opt_edge_set_intp_2( opt_data_2* p, const realType r[ 2 ], unsigned constr )
{
    opt_edge_set_2( p, r, constr );
    opt_edge_intp_2( p );
}

static void opt_edge_hess_2( opt_data_2* p, realType hess[ 2 ] )
{
    unsigned        d;
    const unsigned  de = p->ed.de, n = p->ld[ de ].n;
    const realType* D2 = p->ld[ de ].D2;
    lagrange_2u( &p->ld[ de ] );
    for( d = 0; d < 2; ++d )
        hess[ d ] = tensor_i1( D2, n, p->ed.x[ d ] );
}

static void opt_point_proj_2( opt_data_2* p )
{
    unsigned        off[ 2 ], offt, d, c[ 2 ];
    const realType* fD[ 2 ];
    opt_constr_unpack_2( p->pd.constraints, c );
    for( d = 0; d < 2; ++d )
        if( c[ d ] == 0 )
            fD[ d ] = p->ld[ d ].D_z0, off[ d ] = 0;
        else
            fD[ d ] = p->ld[ d ].D_zn, off[ d ] = p->size[ d + 1 ] - p->size[ d ];
    offt = off[ 0 ] + off[ 1 ];
    for( d = 0; d < 4; ++d )
        p->pd.jac[ d ] = 0;
    for( d = 0; d < 2; ++d )
    {
        unsigned i, j;
        p->pd.x[ d ] = p->elx[ d ][ offt ];
        for( i = 0; i < 2; ++i )
        {
            const realType* in = p->elx[ d ] + offt - off[ i ];
            for( j = 0; j < p->ld[ i ].n; ++j, in += p->size[ i ] )
                p->pd.jac[ d * 2 + i ] += *in * fD[ i ][ j ];
        }
    }
}

static void opt_point_set_2( opt_data_2* p, unsigned constr )
{
    if( p->pd.constraints != constr )
    {
        p->pd.constraints = constr;
        opt_point_proj_2( p );
    }
}

static void opt_point_intp_2( opt_data_2* p )
{
    memcpy( p->x, p->pd.x, 2 * sizeof( realType ) );
    memcpy( p->jac, p->pd.jac, 4 * sizeof( realType ) );
}

static void opt_point_set_intp_2( opt_data_2* p, unsigned constr )
{
    opt_point_set_2( p, constr );
    opt_point_intp_2( p );
}

#define DIAGNOSTICS 0

double opt_findpt_2( opt_data_2* p, const realType* const elx[ 2 ], const realType xstar[ 2 ], realType r[ 2 ],
                     unsigned* constr )
{
    realType dr[ 2 ], resid[ 2 ], steep[ 2 ];

    unsigned c = *constr, ac, d, cc[ 2 ], step = 0;

    p->elx[ 0 ] = elx[ 0 ], p->elx[ 1 ] = elx[ 1 ];

    p->ed.constraints = opt_no_constraints_2;
    p->pd.constraints = opt_no_constraints_2;

#if DIAGNOSTICS
    printf( "opt_findpt: xstar = %g, %g\n", xstar[ 0 ], xstar[ 1 ] );
#endif

    do
    {
        ++step;
        if( step == 50 ) /*fail("%s: opt_findpt_2 did not converge\n",__FILE__);*/
            return 1.e+30;
#if DIAGNOSTICS
        printf( "  iteration %u\n", step );
        printf( "    %d constraint(s) active\n", (int)opt_constr_num_2[ c ] );
#endif
        /* update face/edge/point data if necessary,
           and evaluate x(r) as well as the jacobian */
        switch( opt_constr_num_2[ c ] )
        {
            case 0:
                opt_area_set_intp_2( p, r );
                break;
            case 1:
                opt_edge_set_intp_2( p, r, c );
                break;
            case 2:
                opt_point_set_intp_2( p, c );
                break;
        }
#if DIAGNOSTICS
        printf( "    r = %g, %g\n", r[ 0 ], r[ 1 ] );
        printf( "    x = %g, %g\n", p->x[ 0 ], p->x[ 1 ] );
#endif
        /* compute residual */
        for( d = 0; d < 2; ++d )
            resid[ d ] = xstar[ d ] - p->x[ d ];
#if DIAGNOSTICS
        printf( "    resid = %g, %g\n", resid[ 0 ], resid[ 1 ] );
        printf( "    2-norm = %g\n", r2norm_2( resid[ 0 ], resid[ 1 ] ) );
#endif
        /* check constraints against steepest descent direction */
        ac = c;
        if( opt_constr_num_2[ c ] )
        {
            opt_constr_unpack_2( c, cc );
            mat_app_2c( steep, p->jac, resid ); /* steepest descent = J^T r */
#if DIAGNOSTICS
            printf( "    steepest descent = %g, %g\n", steep[ 0 ], steep[ 1 ] );
#endif
            for( d = 0; d < 2; ++d )
                if( ( cc[ d ] == 0 && steep[ d ] > 0 ) || ( cc[ d ] == 2 && steep[ d ] < 0 ) ) cc[ d ] = 1;
            ac = opt_constr_pack_2( cc );
        }
        /* update face/edge/point data if necessary */
        if( ac != c )
        {
            c = ac;
#if DIAGNOSTICS
            printf( "    relaxed to %d constraints\n", (int)opt_constr_num_2[ c ] );
#endif
            switch( opt_constr_num_2[ c ] )
            {
                case 1:
                    opt_edge_set_2( p, r, c );
                    break;
                case 2:
                    opt_point_set_2( p, c );
                    break;
            }
        }
        /* compute Newton step */
        switch( opt_constr_num_2[ c ] )
        {
            case 0:
                tinyla_solve_2( dr, p->jac, resid );
                break;
            case 1: {
                const unsigned  de = p->ed.de, d1 = p->ed.d1;
                realType        fac, H[ 2 ];
                const realType* J = p->jac + de;
                opt_edge_hess_2( p, H );
                fac = J[ 0 ] * J[ 0 ] + J[ 2 ] * J[ 2 ] - ( resid[ 0 ] * H[ 0 ] + resid[ 1 ] * H[ 1 ] );
                dr[ de ] = steep[ de ] / fac;
                dr[ d1 ] = 0;
            }
            break;
            case 2:
                dr[ 0 ] = dr[ 1 ] = 0;
                break;
        }
#if DIAGNOSTICS
        printf( "    dr = %g, %g\n", dr[ 0 ], dr[ 1 ] );
#endif
        /* project new iteration onto [-1,1]^2 */
        opt_constr_unpack_2( c, cc );
        for( d = 0; d < 2; ++d )
        {
            if( cc[ d ] != 1 ) continue;
            r[ d ] += dr[ d ];
            if( r[ d ] <= -1 )
                dr[ d ] -= r[ d ] + 1, r[ d ] = -1, cc[ d ] = 0;
            else if( r[ d ] >= 1 )
                dr[ d ] -= r[ d ] - 1, r[ d ] = 1, cc[ d ] = 2;
        }
        c = opt_constr_pack_2( cc );
    } while( r1norm_2( dr[ 0 ], dr[ 1 ] ) > 2 * MOAB_POLY_EPS );
    *constr = c;
    return r2norm_2( resid[ 0 ], resid[ 1 ] );
}

#undef DIAGNOSTICS

/*--------------------------------------------------------------------------
   Point Finding  (interface/top-level)

   Initializing the data:
     unsigned nel;        // number of elements
     const unsigned n[3]; // number of nodes in r, s, t directions
     const realType *xm[3];   // n[0]*n[1]*n[2]*nel x,y,z coordinates
     realType tol = 0.01;     // how far point is allowed to be outside element
                          //   relative to element size
     unsigned max_size = n[0]*n[1]*n[2]*nel; // maximum size of hash table

     findpt_data_3 *data = findpt_setup_3(xm,n,nel,max_size,tol);

   Using the data:
     realType x[3] = { ... };   // point to find
     int el; // element number
     realType r[3]; // parametric coordinates
     int guess = 0; // do we use (el,r,s,t) as an initial guess?
     int code; // 0 : normal, -1 : outside all elements,
               // 1 : border, or outside but within tolerance
     realType dist; // distance in xyz space from returned (el,r,s,t) to given
                // (x,y,z)

     code = findpt_3(data, x, guess, &el, r, &dist);

   When done:
     findpt_free_3(&data);

  --------------------------------------------------------------------------*/

/* heap sort on A[0:n-1] with key A[i]->dist
   precondition: n!=0 */
static void findpt_list_sort( findpt_listel** A, unsigned n )
{
    unsigned i;
    --A; /* make A have a base index of 1 */
    /* build heap */
    for( i = 2; i <= n; ++i )
    {
        findpt_listel* item = A[ i ];
        unsigned       hole = i, parent = hole >> 1;
        if( A[ parent ]->dist >= item->dist ) continue;
        do
        {
            A[ hole ] = A[ parent ];
            hole = parent;
            parent >>= 1;
        } while( parent && A[ parent ]->dist < item->dist );
        A[ hole ] = item;
    }
    /* extract */
    for( i = n - 1; i; --i )
    {
        findpt_listel* item = A[ i + 1 ];
        unsigned       hole = 1;
        A[ i + 1 ] = A[ 1 ];
        for( ;; )
        {
            unsigned ch = hole << 1, r = ch + 1;
            if( r <= i && A[ ch ]->dist < A[ r ]->dist ) ch = r;
            if( ch > i || item->dist >= A[ ch ]->dist ) break;
            A[ hole ] = A[ ch ];
            hole = ch;
        }
        A[ hole ] = item;
    }
}

findpt_data_2* findpt_setup_2( const realType* const xw[ 2 ], const unsigned n[ 2 ], uint nel, uint max_hash_size,
                               realType bbox_tol )
{
    unsigned       d;
    findpt_data_2* p = tmalloc( findpt_data_2, 1 );

    p->hash = tmalloc( hash_data_2, 1 );
    p->od = tmalloc( opt_data_2, 1 );

    for( d = 0; d < 2; ++d )
        p->xw[ d ] = xw[ d ];
    p->nptel = n[ 0 ] * n[ 1 ];

    hash_build_2( p->hash, xw, n, nel, max_hash_size, bbox_tol );

    for( d = 0; d < 2; ++d )
    {
        p->z[ d ] = tmalloc( realType, n[ d ] );
        lobatto_nodes( p->z[ d ], n[ d ] );
        lagrange_setup( &p->ld[ d ], p->z[ d ], n[ d ] );
    }

    p->list = tmalloc( findpt_listel, p->hash->max );
    p->sorted = tmalloc( findpt_listel*, p->hash->max );

    opt_alloc_2( p->od, p->ld );
    p->od_work = p->od->work;

    return p;
}

findpt_data_3* findpt_setup_3( const realType* const xw[ 3 ], const unsigned n[ 3 ], uint nel, uint max_hash_size,
                               realType bbox_tol )
{
    unsigned       d;
    findpt_data_3* p = tmalloc( findpt_data_3, 1 );

    p->hash = tmalloc( hash_data_3, 1 );
    p->od = tmalloc( opt_data_3, 1 );

    for( d = 0; d < 3; ++d )
        p->xw[ d ] = xw[ d ];
    p->nptel = n[ 0 ] * n[ 1 ] * n[ 2 ];

    hash_build_3( p->hash, xw, n, nel, max_hash_size, bbox_tol );

    for( d = 0; d < 3; ++d )
    {
        p->z[ d ] = tmalloc( realType, n[ d ] );
        lobatto_nodes( p->z[ d ], n[ d ] );
        lagrange_setup( &p->ld[ d ], p->z[ d ], n[ d ] );
    }

    p->list = tmalloc( findpt_listel, p->hash->max );
    p->sorted = tmalloc( findpt_listel*, p->hash->max );

    opt_alloc_3( p->od, p->ld );
    p->od_work = p->od->work;

    return p;
}

void findpt_free_2( findpt_data_2* p )
{
    unsigned d;
    opt_free_2( p->od );
    free( p->od );
    hash_free_2( p->hash );
    free( p->hash );
    free( p->list );
    free( p->sorted );
    for( d = 0; d < 2; ++d )
        free( p->z[ d ] );
    free( p );
}

void findpt_free_3( findpt_data_3* p )
{
    unsigned d;
    opt_free_3( p->od );
    free( p->od );
    hash_free_3( p->hash );
    free( p->hash );
    free( p->list );
    free( p->sorted );
    for( d = 0; d < 3; ++d )
        free( p->z[ d ] );
    free( p );
}

const realType* findpt_allbnd_2( const findpt_data_2* p )
{
    return p->hash->bnd;
}

const realType* findpt_allbnd_3( const findpt_data_3* p )
{
    return p->hash->bnd;
}

static void findpt_hash_2( findpt_data_2* p, const realType x[ 2 ] )
{
    findpt_listel *list = p->list, **sorted = p->sorted;
    const uint     hi = hash_index_2( p->hash, x );
    const uint*    offset = p->hash->offset;
    uint           i;
    const uint     b = offset[ hi ], e = offset[ hi + 1 ];
    for( i = b; i != e; ++i )
    {
        const uint     el = offset[ i ];
        realType*      r = &list->r[ 0 ];
        const obbox_2* obb = &p->hash->obb[ el ];
        if( obbox_axis_test_2( obb, x ) ) continue;
        if( obbox_test_2( obb, x, r ) ) continue;
        list->el = el;
        list->dist = r1norm_2( r[ 0 ], r[ 1 ] );
        *sorted++ = list++;
    }
    p->end = sorted;
    if( p->end != p->sorted ) findpt_list_sort( p->sorted, p->end - p->sorted );
}

static void findpt_hash_3( findpt_data_3* p, const realType x[ 3 ] )
{
    findpt_listel *list = p->list, **sorted = p->sorted;
    const uint     hi = hash_index_3( p->hash, x );
    const uint*    offset = p->hash->offset;
    uint           i;
    const uint     b = offset[ hi ], e = offset[ hi + 1 ];
    for( i = b; i != e; ++i )
    {
        const uint     el = offset[ i ];
        realType*      r = &list->r[ 0 ];
        const obbox_3* obb = &p->hash->obb[ el ];
        if( obbox_axis_test_3( obb, x ) ) continue;
        if( obbox_test_3( obb, x, r ) ) continue;
        list->el = el;
        list->dist = r1norm_3( r[ 0 ], r[ 1 ], r[ 2 ] );
        *sorted++ = list++;
    }
    p->end = sorted;
    if( p->end != p->sorted ) findpt_list_sort( p->sorted, p->end - p->sorted );
}

static int findpt_guess_2( findpt_data_2* p, const realType x[ 2 ], uint el, realType r[ 2 ], realType* dist )
{
    const uint      arrindex = p->nptel * el;
    const realType* elx[ 2 ];
    realType        g[ 2 ];
    unsigned        c = opt_no_constraints_2;
    const obbox_2*  obb = &p->hash->obb[ el ];
    elx[ 0 ] = p->xw[ 0 ] + arrindex;
    elx[ 1 ] = p->xw[ 1 ] + arrindex;
    if( obbox_axis_test_2( obb, x ) || obbox_test_2( obb, x, g ) ) return 0;
    *dist = opt_findpt_2( p->od, elx, x, r, &c );
    return c == opt_no_constraints_2;
}

static int findpt_guess_3( findpt_data_3* p, const realType x[ 3 ], uint el, realType r[ 3 ], realType* dist )
{
    const uint      arrindex = p->nptel * el;
    const realType* elx[ 3 ];
    realType        g[ 3 ];
    unsigned        c = opt_no_constraints_3;
    const obbox_3*  obb = &p->hash->obb[ el ];
    elx[ 0 ] = p->xw[ 0 ] + arrindex;
    elx[ 1 ] = p->xw[ 1 ] + arrindex;
    elx[ 2 ] = p->xw[ 2 ] + arrindex;
    if( obbox_axis_test_3( obb, x ) || obbox_test_3( obb, x, g ) ) return 0;
    *dist = opt_findpt_3( p->od, elx, x, r, &c );
    return c == opt_no_constraints_3;
}

#define DIAGNOSTICS 0

static int findpt_pass_2( findpt_data_2* p, const realType x[ 2 ], uint* el, realType r[ 2 ], realType* dist_min )
{
    findpt_listel** qq = p->sorted;
    const realType* bnd;
    realType        dist;
    do
    {
        findpt_listel*  q = *qq;
        const uint      arrindex = p->nptel * q->el;
        const realType* elx[ 2 ];
        unsigned        c = opt_no_constraints_2;
        elx[ 0 ] = p->xw[ 0 ] + arrindex;
        elx[ 1 ] = p->xw[ 1 ] + arrindex;
        dist = opt_findpt_2( p->od, elx, x, q->r, &c );
        if( qq == p->sorted || dist < *dist_min || c == opt_no_constraints_2 )
        {
            *dist_min = dist;
            *el = q->el;
            memcpy( r, q->r, 2 * sizeof( realType ) );
            if( c == opt_no_constraints_2 ) return 0;
        }
    } while( ++qq != p->end );
    bnd = p->hash->obb[ *el ].axis_bnd;
    return *dist_min > r2norm_2( bnd[ 1 ] - bnd[ 0 ], bnd[ 3 ] - bnd[ 2 ] ) ? -1 : 1;
}

static int findpt_pass_3( findpt_data_3* p, const realType x[ 3 ], uint* el, realType r[ 3 ], realType* dist_min )
{
    findpt_listel** qq = p->sorted;
    const realType* bnd;
    realType        dist;
    do
    {
        findpt_listel*  q = *qq;
        const uint      arrindex = p->nptel * q->el;
        const realType* elx[ 3 ];
        unsigned        c = opt_no_constraints_3;
        elx[ 0 ] = p->xw[ 0 ] + arrindex;
        elx[ 1 ] = p->xw[ 1 ] + arrindex;
        elx[ 2 ] = p->xw[ 2 ] + arrindex;
        dist = opt_findpt_3( p->od, elx, x, q->r, &c );
        if( qq == p->sorted || dist < *dist_min || c == opt_no_constraints_3 )
        {
            *dist_min = dist;
            *el = q->el;
            memcpy( r, q->r, 3 * sizeof( realType ) );
            if( c == opt_no_constraints_3 )
            {
#if DIAGNOSTICS
                printf( "point found in element #%d\n", qq - p->sorted );
#endif
                return 0;
            }
        }
    } while( ++qq != p->end );
    bnd = p->hash->obb[ *el ].axis_bnd;
    return *dist_min > r2norm_3( bnd[ 1 ] - bnd[ 0 ], bnd[ 3 ] - bnd[ 2 ], bnd[ 5 ] - bnd[ 4 ] ) ? -1 : 1;
}

int findpt_2( findpt_data_2* p, const realType x[ 2 ], int guess, uint* el, realType r[ 2 ], realType* dist )
{
    if( guess && findpt_guess_2( p, x, *el, r, dist ) ) return 0;
    findpt_hash_2( p, x );
    if( p->sorted == p->end ) return -1;
    return findpt_pass_2( p, x, el, r, dist );
}

int findpt_3( findpt_data_3* p, const realType x[ 3 ], int guess, uint* el, realType r[ 3 ], realType* dist )
{
    if( guess && findpt_guess_3( p, x, *el, r, dist ) ) return 0;
    findpt_hash_3( p, x );
#if DIAGNOSTICS
    printf( "hashing leaves %d elements to consider\n", p->end - p->sorted );
#endif
    if( p->sorted == p->end ) return -1;
    return findpt_pass_3( p, x, el, r, dist );
}

void findpt_weights_2( findpt_data_2* p, const realType r[ 2 ] )
{
    lagrange_0( &p->ld[ 0 ], r[ 0 ] );
    lagrange_0( &p->ld[ 1 ], r[ 1 ] );
}

void findpt_weights_3( findpt_data_3* p, const realType r[ 3 ] )
{
    lagrange_0( &p->ld[ 0 ], r[ 0 ] );
    lagrange_0( &p->ld[ 1 ], r[ 1 ] );
    lagrange_0( &p->ld[ 2 ], r[ 2 ] );
}

double findpt_eval_2( findpt_data_2* p, const realType* u )
{
    return tensor_i2( p->ld[ 0 ].J, p->ld[ 0 ].n, p->ld[ 1 ].J, p->ld[ 1 ].n, u, p->od_work );
}

double findpt_eval_3( findpt_data_3* p, const realType* u )
{
    return tensor_i3( p->ld[ 0 ].J, p->ld[ 0 ].n, p->ld[ 1 ].J, p->ld[ 1 ].n, p->ld[ 2 ].J, p->ld[ 2 ].n, u,
                      p->od_work );
}
